Double-antenna GNSS (Global Navigation Satellite System)-assisted heading attitude measurement method, equipment and system

Abstract

The invention provides a double-antenna GNSS (Global Navigation Satellite System)-assisted navigation attitude measurement method. Real-time high-precision navigation positioning orientation is realized through the following steps of: constructing a coordinate system projection matrix by utilizing the height difference of double antennas; calculating course information delay time of satellite navigation and inertial navigation, and caching inertial navigation data; finding an inertial navigation course at a corresponding moment in the cache according to the delay time, and converting a coordinate system through a projection matrix; comparing the converted inertial navigation course with the satellite navigation course, and calculating an error; after effectiveness judgment is carried out on the error, a Kalman filter is adopted to carry out filtering iteration, and optimal estimation of an error term is obtained; and correcting the original navigation result according to the filtering result.

Description

Technical Field

[0001] The present invention relates to the field of shipborne navigation technology, and in particular to a dual-antenna GNSS-assisted heading attitude measurement method, equipment and system. Background Art

[0002] When shipborne optoelectronics performs target positioning, the attitude reference system is required to provide it with real-time, high-precision positioning and orientation information. The optoelectronic system measures the azimuth and pitch angle of the target measured in the deck coordinate system, and uses the attitude reference information to convert the azimuth and distance of the target in the geographic coordinate system. Combined with the position information of the attitude reference system, the position of the target in the geographic coordinate system is finally calculated. Therefore, the real-time positioning accuracy and orientation accuracy of the attitude reference system are the key factors affecting shipborne optoelectronic target positioning.

[0003] In the attitude reference system, inertial navigation and satellite navigation are the two most commonly used navigation methods. Inertial navigation has the advantages of comprehensive output information, high output frequency and high short-term accuracy. However, a significant disadvantage of inertial navigation is that the positioning error will accumulate rapidly over time. On the other hand, satellite navigation, such as the Global Navigation Satellite System (GNSS), can provide accurate navigation and positioning information, but its navigation information output frequency is relatively low, and the signal often contains large noise.

[0004] In order to combine the respective advantages of inertial navigation and satellite navigation and achieve real-time high-precision navigation and positioning, the industry generally adopts the method of combining the two for navigation. However, in practical applications, this combined navigation method also faces a series of technical challenges. Especially for low-cost inertial navigation systems, it is difficult to achieve high-precision autonomous orientation due to the limited accuracy of their sensors. At the same time, when dual-antenna satellite navigation is used for orientation measurement, although the orientation accuracy can be improved by increasing the dual-antenna baseline length (such as when the baseline length is greater than 2 meters, the heading accuracy can be better than 0.1°), the antenna position is limited, especially when there is a height difference, the dual-antenna heading cannot accurately represent the actual heading of the vehicle when it is swaying. In addition, the dual-antenna GNSS heading solution delay is large, which is difficult to meet application scenarios such as optoelectronic systems that have strict requirements for dynamic real-time target positioning. Summary of the invention

[0005] The purpose of the present invention is to provide a dual-antenna GNSS-assisted heading measurement method to address the problems existing in the prior art, thereby achieving real-time high-precision navigation positioning and orientation by compensating for the height difference between the dual antennas and the satellite navigation solution delay.

[0006] To achieve the above object, the technical solution adopted by the present invention is:

[0007] A dual-antenna GNSS-assisted heading measurement method, comprising:

[0008] The projection matrix from the hull coordinate system to the coordinate system where the satellite navigation dual antennas are located is constructed using the height difference of the dual antennas;

[0009] Calculate the delay time ΔT of the satellite navigation heading information relative to the inertial navigation heading information;

[0010] Use inertial navigation to measure the heading information, and cache the heading information and the corresponding timestamp into an array;

[0011] In the satellite navigation heading update cycle, according to the delay time ΔT, the inertial navigation heading information at the time corresponding to the satellite navigation heading information is found in the cached inertial navigation data;

[0012] The found inertial navigation heading information is converted from the hull coordinate system to the satellite navigation dual antenna coordinate system through the projection matrix;

[0013] Compare the converted inertial navigation heading information with the satellite navigation heading information, calculate the difference between them, and obtain the errors of position, speed and heading as error measurement values;

[0014] The validity of the heading error measurement value is judged. If it is valid, the next Kalman filter update step is performed;

[0015] The Kalman filter is used to construct the state estimation model and the measurement model. The state estimation is filtered and iterated through the error measurement value to obtain the optimal estimate of each error term at the current moment.

[0016] According to the output of the Kalman filter, the original navigation results are corrected.

[0017] Construct the projection matrix from the hull coordinate system to the coordinate system of the satellite navigation dual antenna, including:

[0018] According to the hull structure model or actual on-site measurement, determine the vertical distance H and longitudinal distance L between the main antenna and the slave antenna in the hull coordinate system;

[0019] According to the vertical distance H and the longitudinal distance L, the pitch angle deviation Δθ between the satellite navigation dual antenna coordinate system and the hull coordinate system is calculated. The formula is as follows:

[0020]

[0021] According to the calculated pitch angle deviation Δθ, the projection matrix from the hull coordinate system to the satellite navigation dual antenna coordinate system is constructed as follows:

[0022]

[0023] Calculate the delay time ΔT of the satellite navigation heading information relative to the inertial navigation heading information, including:

[0024] Without the assistance of dual-antenna heading information and in dynamic conditions, simultaneously obtain satellite navigation heading information, inertial navigation heading information and heading angular rate information;

[0025] Convert the heading information measured by inertial navigation into the coordinate system where the satellite navigation antenna is located;

[0026] Calculate the deviation between the converted inertial navigation heading and the heading measured by the satellite navigation antenna, denoted as Δyaw;

[0027] Obtain the heading angular velocity from the inertial navigation data, denoted as Δw;

[0028] Using the least squares method, the delay time ΔT of the satellite navigation heading information is calculated based on the heading deviation Δyaw and the heading angular velocity Δw of the inertial navigation. The calculation formula is as follows:

[0029]

[0030] According to the delay time ΔT, the inertial navigation heading information at the time corresponding to the satellite navigation heading information is found in the cached inertial navigation data, including:

[0031] Traverse the cached inertial navigation data array, compare the timestamp of each data point with the timestamp of the current satellite navigation heading information, and find the data point with the closest time difference to ΔT as the matching object.

[0032] Build a state estimation model, including:

[0033] Define the system state vector X as follows,

[0034]

[0035] Among them, δp is the horizontal position error, δv is the horizontal velocity error, is the attitude error, δb a is the accelerometer bias, δb g is the gyro bias, δk g is the gyroscope scale coefficient error;

[0036] Establish the state transfer equation as follows:

[0037] X k =ψ k，k-1 X k-1 +W k ;

[0038] Among them, X k is the state vector at the current time k, X k-1 is the state vector at the previous time k-1, ψk，k-1 is the state transfer matrix, W k is the system noise matrix;

[0039] Build a measurement model, including:

[0040] Define the quantity measurement Z as follows,

[0041] Z = [δp δv δy];

[0042] Among them, δp is the horizontal position error, δv is the horizontal velocity error, and δy is the heading angle error;

[0043] The measurement equation is established as follows:

[0044] Z k =H k X k +V k ;

[0045] Among them, Z k is the quantity measurement at the current time k, X k is the system state vector at the current time k, H k is the measurement matrix, V k is the measurement noise matrix.

[0046] The state estimation is filtered and iterated through the error measurement value to obtain the optimal estimate of each error term at the current moment, including:

[0047] Set the initial estimate of the state vector X and initialize the covariance matrix of the state vector;

[0048] Set the process noise covariance matrix and the measurement noise covariance matrix;

[0049] At the beginning of each filtering cycle, the state transfer equation and the state vector estimate at the previous moment are used to predict the prior estimate of the state vector at the current moment, and at the same time, the prior estimate of the covariance matrix is ​​updated;

[0050] Substitute the heading error measurement value and use the measurement equation to calculate the measurement residual;

[0051] The Kalman gain is calculated based on the measurement residual, the prior estimate of the covariance matrix, and the measurement noise covariance matrix;

[0052] Use the Kalman gain to update the posterior estimate of the state vector and the posterior estimate of the covariance matrix;

[0053] From the updated state vector, estimates of the horizontal position error, horizontal velocity error, and heading angle error are extracted.

[0054] The validity of the heading error measurement value is judged, including:

[0055] The real baseline length L1 of the dual-antenna GNSS is measured using a measuring device, and the current baseline length L2 is calculated in real time using an algorithm within the navigation system. The baseline length deviation is calculated as ΔL=L2-L1;

[0056] The heading data measured by the dual-antenna GNSS is continuously recorded over a period of time, and the standard deviation σyaw of the heading data over this period of time is calculated using the standard deviation formula;

[0057] Set the baseline length deviation threshold ΔL1 and the dual-antenna heading standard deviation threshold σyaw1. Before each measurement update, check whether the current baseline length deviation ΔL is less than ΔL1 and whether the dual-antenna heading standard deviation σyaw is less than σyaw1.

[0058] If both conditions are met, the current heading error measurement is considered valid and the subsequent Kalman filter update step can be performed.

[0059] A dual-antenna GNSS-assisted heading measurement device, comprising:

[0060] An inertial navigation component, the inertial component includes a gyroscope, an accelerometer, a power module, an IF module, a satellite navigation solution module and a navigation solution module; the gyroscope is used to measure the angular velocity of the hull and provide heading angular rate information; the accelerometer is used to measure the acceleration of the hull and assist in navigation solution; the power module is used to provide a stable power supply for the inertial component; the IF module is used to realize data communication of the inertial component; the satellite navigation solution module is used to receive and process satellite navigation signals and calculate satellite navigation heading information; the navigation solution module combines the data of the gyroscope and accelerometer with the satellite navigation information to perform navigation solution and provide initial heading, position and speed information;

[0061] The dual antenna assembly, including a main antenna and a slave antenna, is used to receive satellite navigation signals and calculate satellite navigation heading information through dual antenna technology.

[0062] A dual-antenna GNSS-aided heading and attitude measurement system, comprising:

[0063] The projection matrix calculation module uses the height difference of the dual antennas to construct the projection matrix from the hull coordinate system to the coordinate system where the dual satellite navigation antennas are located;

[0064] A delay time calculation module, used to calculate the delay time ΔT of the satellite navigation heading information relative to the inertial navigation heading information;

[0065] The data cache module is used to cache the heading information measured by the inertial navigation and the corresponding timestamp into an array;

[0066] The data synchronization module is used to find the inertial navigation heading information at the time corresponding to the satellite navigation heading information in the cached inertial navigation data according to the delay time ΔT during the satellite navigation heading update cycle;

[0067] A coordinate conversion module is used to convert the found inertial navigation heading information from the hull coordinate system to the satellite navigation dual antenna coordinate system through a projection matrix;

[0068] The error calculation module is used to compare the converted inertial navigation heading information with the satellite navigation heading information and calculate the difference between them, that is, the heading error measurement value;

[0069] A validity judgment module is used to judge the validity of the heading error measurement value;

[0070] The Kalman filter module is used to construct a state estimation model and a measurement model when the heading error measurement value is valid, and to perform filtering iteration on the state estimation through the error measurement value to obtain the optimal estimate of each error term at the current moment.

[0071] A computer-readable storage medium stores a computer program, wherein the computer program implements the above method steps when executed by a processor.

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

[0073] 1. When inertial navigation and satellite navigation are combined, the influence of the height difference of dual antennas on navigation accuracy is taken into consideration. By constructing a projection matrix, the inertial navigation data is converted into the dual antenna coordinate system of satellite navigation, thereby compensating for the heading error caused by the antenna height difference and ensuring a high heading accuracy.

[0074] 2. By calculating the delay time of satellite navigation heading information relative to inertial navigation heading information and performing time alignment processing, the impact of satellite navigation heading solution delay on navigation accuracy is effectively reduced, and the precise synchronization of the two navigation information in time is achieved;

[0075] 3. The Kalman filter is used to estimate and correct the heading error, so that the system can still maintain high navigation accuracy and stability in the face of complex navigation environments and noise interference;

[0076] 4. Through time alignment and real-time processing of the Kalman filter, the navigation results can be corrected in real time to meet the requirements of dynamic real-time target positioning. BRIEF DESCRIPTION OF THE DRAWINGS

[0077] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0078] Figure 1 This is a flow chart of the dual-antenna GNSS-assisted heading measurement method in an embodiment of the present application;

[0079] Figure 2 This is a structural diagram of the dual antenna installation position in an embodiment of the present application. DETAILED DESCRIPTION

[0080] The technical solution of the present invention will be described clearly and completely below in conjunction with the accompanying drawings of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0081] The following is an explanation of some terms in this application. It should be noted that these explanations are for the convenience of understanding by those skilled in the art and are not intended to limit the scope of protection claimed in this application.

[0082] The order of execution of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of the present application.

[0083] In the description of the present specification and the appended claims, the terms "first", "second", "third", etc. are only used to distinguish descriptions and cannot be understood as indicating or suggesting relative importance. It should also be understood that although the terms "first", "second", etc. are used to describe various elements in some embodiments of the present application in the text, these elements should not be limited by these terms. These terms are only used to distinguish one element from another element. For example, the first table can be named as the second table, and similarly, the second table can be named as the first table without departing from the scope of the various described embodiments. The first table and the second table are both tables, but they are not the same table.

[0084] References to "one embodiment" or "some embodiments" etc. described in the specification of this application mean that one or more embodiments of the present application include specific features, structures or characteristics described in conjunction with the embodiment. Therefore, the statements "in one embodiment", "in some embodiments", "in some other embodiments", "in some other embodiments", etc. that appear in different places in this specification do not necessarily refer to the same embodiment, but mean "one or more but not all embodiments", unless otherwise specifically emphasized in other ways. The terms "including", "comprising", "having" and their variations all mean "including but not limited to", unless otherwise specifically emphasized in other ways.

[0085] Inertial navigation has the advantages of complete output information, high output frequency, and high short-term accuracy, but positioning errors accumulate rapidly over time. Satellite navigation can provide accurate navigation and positioning information, but its navigation information output frequency is low and there is a lot of noise. After combined navigation, inertial navigation and satellite navigation can complement each other to achieve real-time high-precision navigation and positioning.

[0086] However, when inertial navigation and satellite navigation are combined, low-cost inertial navigation is difficult to achieve high-precision autonomous orientation due to the limitations of sensor accuracy. The orientation measurement accuracy of dual-antenna satellite navigation is related to the length of the dual-antenna baseline. When the baseline length is greater than 2m, a heading accuracy better than 0.1° can be achieved. However, when the antenna position is limited and there is a height difference, the dual-antenna heading cannot represent the actual heading of the vehicle when it is swaying, and the dual-antenna GNSS heading solution delay is large, which is difficult to meet the requirements of dynamic real-time target positioning of the optoelectronic system.

[0087] Regarding the above technical issues, such as Figure 1 As shown, the embodiment of the present application provides a dual-antenna GNSS-assisted heading measurement method, comprising the following steps:

[0088] S1. Use the height difference of the dual antennas to construct a projection matrix from the hull coordinate system to the coordinate system where the dual satellite navigation antennas are located;

[0089] S2, calculating the delay time ΔT of the satellite navigation heading information relative to the inertial navigation heading information;

[0090] S3, obtain the heading information measured by the inertial navigation, and cache the information together with the corresponding timestamp into an array;

[0091] S4. In the satellite navigation heading update cycle, according to the delay time ΔT, find the inertial navigation heading information at the time corresponding to the current satellite navigation heading information;

[0092] S5. Convert the found inertial navigation heading information from the hull coordinate system to the satellite navigation dual antenna coordinate system through the projection matrix;

[0093] S6. Compare the converted inertial navigation heading information with the satellite navigation heading information, calculate the difference between them, and obtain the errors of position, speed and heading as error measurement values;

[0094] S7, judging the validity of the error measurement value, if it is valid, proceeding to the next Kalman filter update step;

[0095] S8. Use the Kalman filter to construct a state estimation model and a measurement model, perform filtering iterations on the state estimation through the error measurement value, and obtain the optimal estimate of each error term at the current moment;

[0096] S9. According to the output result of the Kalman filter, the original navigation result is corrected.

[0097] The above method of this embodiment realizes real-time high-precision navigation and positioning by combining the advantages of inertial navigation and GNSS dual antennas.

[0098] Since the data provided by inertial navigation is based on the hull coordinate system, while the satellite navigation data is based on its dual antenna coordinate system, when inertial navigation and satellite navigation are combined, the influence of the height difference of the dual antennas on navigation accuracy is taken into consideration. By constructing a projection matrix, the inertial navigation data is converted to the dual antenna coordinate system of satellite navigation, thereby compensating for the heading error caused by the antenna height difference and ensuring a high heading accuracy.

[0099] Since there may be a delay in updating the heading information of satellite navigation, while inertial navigation provides navigation information in real time, this embodiment first calculates the delay time ΔT between the two types of navigation information, and performs time alignment processing based on the delay time, effectively reducing the impact of the satellite navigation heading solution delay on navigation accuracy.

[0100] The Kalman filter is used to estimate and correct the heading error, so that high navigation accuracy can be maintained even in the face of complex navigation environments and noise interference.

[0101] Through time alignment and real-time processing of the Kalman filter, real-time navigation result correction can be achieved to meet the requirements of dynamic real-time target positioning.

[0102] Specifically, in step S1, the projection matrix from the hull coordinate system to the coordinate system where the satellite navigation dual antennas are located is constructed using the height difference of the dual antennas, including:

[0103] S1.1. Determine the vertical distance H and longitudinal distance L between the main antenna and the slave antenna in the hull coordinate system based on the hull structure model or actual on-site measurement;

[0104] S1.2. Calculate the pitch angle deviation Δθ between the satellite navigation dual antenna coordinate system and the hull coordinate system based on the vertical distance H and the longitudinal distance L. The pitch angle deviation reflects the relative inclination of the two coordinate systems in the vertical direction. The formula is as follows:

[0105]

[0106] S1.3. Based on the calculated pitch angle deviation Δθ, a projection matrix from the hull coordinate system to the satellite navigation dual antenna coordinate system is constructed to achieve data conversion between the two coordinate systems, as follows:

[0107]

[0108] Through the above steps, the constructed projection matrix is ​​used as a conversion tool to realize the conversion of data from the hull coordinate system to the satellite navigation dual-antenna coordinate system, and compensate for the influence of the height difference of the dual antennas.

[0109] Specifically, in step S2, calculating the delay time ΔT of the satellite navigation heading information relative to the inertial navigation heading information includes:

[0110] S2.1. Without using dual-antenna heading information assistance and in dynamic conditions, simultaneously obtain satellite navigation heading information, inertial navigation heading information and heading angular rate information;

[0111] S2.2, converting the heading information measured by the inertial navigation into the coordinate system where the satellite navigation antenna is located;

[0112] S2.3, calculate the deviation between the converted inertial navigation heading and the heading measured by the satellite navigation antenna, denoted as Δyaw;

[0113] S2.4, obtain the heading angular velocity from the inertial navigation data, denoted as Δw;

[0114] S2.5. Use the least squares method to calculate the delay time ΔT of the satellite navigation heading information based on the heading deviation Δyaw and the heading angular velocity Δw of the inertial navigation. The calculation formula is as follows:

[0115]

[0116] The least square method is an optimization method used to find the optimal delay time ΔT so that the deviation between the satellite navigation heading information and the inertial navigation heading information is minimized under this delay time.

[0117] Through the above steps, the delay time ΔT is calculated, which facilitates the subsequent precise synchronization of satellite navigation and inertial navigation in time.

[0118] Specifically, in step S4, according to the delay time ΔT, the inertial navigation heading information at the time corresponding to the satellite navigation heading information is found in the cached inertial navigation data, including:

[0119] Traverse the cached inertial navigation data array, compare the timestamp of each data point with the timestamp of the current satellite navigation heading information, and find the data point with the closest time difference to ΔT as the matching object.

[0120] In practical applications, the calculation delay of dual-antenna GNSS heading is relatively large, and there is a time difference between the inertial navigation and satellite navigation data. This embodiment effectively eliminates this time difference by calculating the delay time ΔT and performing time synchronization based on it.

[0121] Specifically, in step S8, constructing a state estimation model includes:

[0122] Define the system state vector X as follows,

[0123]

[0124] Among them, δp is the horizontal position error, δv is the horizontal velocity error, is the attitude error, δb a is the accelerometer bias, δb g is the gyro bias, δk g is the gyroscope scale coefficient error;

[0125] Establish the state transfer equation as follows:

[0126] X k =ψ k，k-1 X k-1 +W k ;

[0127] Among them, X k is the state vector at the current time k, X k-1 is the state vector at the previous time k-1, ψ k，k-1 is the state transfer matrix, W k is the system noise matrix.

[0128] Specifically, in step S8, constructing a measurement model includes:

[0129] Define the quantity measurement Z as follows,

[0130] Z = [δp δv δy];

[0131] Among them, δp is the horizontal position error, δv is the horizontal velocity error, and δy is the heading angle error;

[0132] The measurement equation is established as follows:

[0133] Z k =H k X k +V k ;

[0134] Among them, Z k is the quantity measurement at the current time k, X k is the system state vector at the current time k, H k is the measurement matrix, V k is the measurement noise matrix.

[0135] Specifically, in step S8, the state estimation is filtered and iterated by the error measurement value to obtain the optimal estimate of each error term at the current moment, including:

[0136] S8.1. Set the initial estimate of the state vector and initialize the covariance matrix of the state vector;

[0137] S8.2, setting the process noise covariance matrix and the measurement noise covariance matrix;

[0138] S8.3, at the beginning of each filtering cycle (consistent with the update cycle of satellite navigation), the state transfer equation and the state vector estimate at the previous moment are used to predict the prior estimate of the state vector at the current moment; at the same time, the prior estimate of the covariance matrix is ​​updated according to the state transfer matrix and the posterior estimate of the covariance matrix at the previous moment;

[0139] S8.4, when new error measurements (position, velocity, and heading) are received, the measurement residuals are calculated using the measurement equations;

[0140] S8.5, calculating the Kalman gain based on the measurement residual, the prior estimate of the covariance matrix, and the measurement noise covariance matrix;

[0141] S8.6, using the Kalman gain to update the posterior estimate of the state vector and the posterior estimate of the covariance matrix;

[0142] S8.7. Extract estimates of the horizontal position error, horizontal velocity error, and heading angle error from the updated state vector.

[0143] In step S9, the original navigation result is corrected according to the output result of the Kalman filter.

[0144] Exemplarily, the estimated values ​​of the horizontal position error and the velocity error are added to the original position and velocity data respectively to obtain more accurate position and velocity information; the estimated value of the heading angle error is added to the original heading angle to obtain more accurate heading information.

[0145] Specifically, in step S7, the validity of the heading error measurement value is judged, including:

[0146] S7.1. Use the measuring equipment to measure the real baseline length L1 of the dual-antenna GNSS, calculate the current baseline length L2 in real time through the algorithm inside the navigation system, and calculate the baseline length deviation value as ΔL=L2-L1;

[0147] S7.2. Continuously record the heading data measured by the dual-antenna GNSS for a period of time (several minutes or tens of seconds), and use the standard deviation formula to calculate the standard deviation σyaw of the heading data during this period;

[0148] S7.3, set the baseline length deviation threshold ΔL1 and the dual-antenna heading standard deviation threshold σyaw1, and before each measurement update, check whether the current baseline length deviation ΔL is less than ΔL1, and whether the dual-antenna heading standard deviation σyaw is less than σyaw1;

[0149] S7.4. If both conditions are met, the current heading error measurement value is considered valid and the subsequent Kalman filter update step can be performed;

[0150] S7.5. If any of the conditions is not met, the current heading error measurement value is considered to be invalid and the use of the measurement value for Kalman filter updating should be suspended.

[0151] like Figure 2 As shown, in a second aspect of an embodiment of the present application, a dual-antenna GNSS-assisted heading measurement device is provided, comprising:

[0152] An inertial navigation component, the inertial component includes a gyroscope, an accelerometer, a power module, an IF module, a satellite navigation solution module and a navigation solution module; the gyroscope is used to measure the angular velocity of the hull and provide heading angular rate information; the accelerometer is used to measure the acceleration of the hull and assist in navigation solution; the power module is used to provide a stable power supply for the inertial component; the IF module is used to realize data communication of the inertial component; the satellite navigation solution module is used to receive and process satellite navigation signals and calculate satellite navigation heading information; the navigation solution module combines the data of the inertial navigation component and the satellite navigation solution module, performs fusion solution, and provides initial heading, position and speed information;

[0153] The dual antenna assembly, including a main antenna and a slave antenna, is used to receive satellite navigation signals and calculate satellite navigation heading information through dual antenna technology.

[0154] According to a third aspect of the embodiments of the present application, a dual-antenna GNSS-assisted heading and attitude measurement system is provided, comprising:

[0155] The projection matrix calculation module is used to construct the projection matrix from the hull coordinate system to the satellite navigation dual antenna coordinate system;

[0156] A delay time calculation module, used to calculate the delay time ΔT of the satellite navigation heading information relative to the inertial navigation heading information;

[0157] The data cache module is used to cache the heading information measured by the inertial navigation and the corresponding timestamp into an array;

[0158] The data synchronization module is used to find the inertial navigation heading information at the time corresponding to the satellite navigation heading information in the cached inertial navigation data according to the delay time ΔT during the satellite navigation heading update cycle;

[0159] A coordinate conversion module is used to convert the found inertial navigation heading information from the hull coordinate system to the satellite navigation dual antenna coordinate system through a projection matrix;

[0160] The error calculation module is used to compare the converted inertial navigation heading information with the satellite navigation heading information and calculate the difference between them, that is, the heading error measurement value;

[0161] A validity judgment module is used to judge the validity of the heading error measurement value;

[0162] The Kalman filter module is used to construct a state estimation model and a measurement model when the heading error measurement value is valid, bring in the heading error measurement value, obtain the optimal estimate of the heading attitude at the current moment through filtering iteration, and output the corrected navigation result.

[0163] According to a fourth aspect of an embodiment of the present application, a computer-readable storage medium is provided, in which a computer program is stored. When the computer program is executed by a processor, the above-mentioned method steps are implemented.

[0164] Although embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions and variations may be made to the embodiments without departing from the principles and spirit of the present invention, and that the scope of the present invention is defined by the appended claims and their equivalents.

Claims

1. A dual-antenna GNSS-assisted heading measurement method, characterized in that: include: The projection matrix from the hull coordinate system to the satellite navigation dual antenna coordinate system is constructed using the height difference of the dual antennas; Calculate the delay time ΔT of the satellite navigation heading information relative to the inertial navigation heading information; Use inertial navigation to measure the heading information, and cache the heading information and the corresponding timestamp into an array; In the satellite navigation heading update cycle, according to the delay time ΔT, the inertial navigation heading information at the time corresponding to the satellite navigation heading information is found in the cached inertial navigation data; The found inertial navigation heading information is converted from the hull coordinate system to the satellite navigation dual antenna coordinate system through the projection matrix; Compare the converted inertial navigation heading information with the satellite navigation heading information, calculate the difference between them, and obtain the errors of position, speed and heading as error measurement values; The validity of the heading error measurement value is judged. If it is valid, the next Kalman filter update step is performed; The Kalman filter is used to construct the state estimation model and the measurement model. The state estimation is filtered and iterated through the error measurement value to obtain the optimal estimate of each error term at the current moment. According to the output of the Kalman filter, the original navigation results are corrected.

2. A dual-antenna GNSS-assisted heading measurement method according to claim 1, characterized in that: Construct the projection matrix from the hull coordinate system to the coordinate system of the satellite navigation dual antenna, including: According to the hull structure model or actual on-site measurement, determine the vertical distance H and longitudinal distance L between the main antenna and the slave antenna in the hull coordinate system; According to the vertical distance H and the longitudinal distance L, the pitch angle deviation Δθ between the satellite navigation dual antenna coordinate system and the hull coordinate system is calculated. The formula is as follows: According to the calculated pitch angle deviation Δθ, the projection matrix from the hull coordinate system to the satellite navigation dual antenna coordinate system is constructed as follows:

3. A dual-antenna GNSS-assisted heading measurement method according to claim 1, characterized in that: Calculate the delay time ΔT of the satellite navigation heading information relative to the inertial navigation heading information, including: Without the assistance of dual-antenna heading information and in dynamic conditions, simultaneously obtain satellite navigation heading information, inertial navigation heading information and heading angular rate information; Convert the heading information measured by inertial navigation into the coordinate system where the satellite navigation antenna is located; Calculate the deviation between the converted inertial navigation heading and the heading measured by the satellite navigation antenna, denoted as Δyaw; Obtain the heading angular velocity from the inertial navigation data, denoted as Δw; Using the least squares method, the delay time ΔT of the satellite navigation heading information is calculated based on the heading deviation Δyaw and the heading angular velocity Δw of the inertial navigation. The calculation formula is as follows:

4. The dual-antenna GNSS-assisted heading measurement method according to claim 1, characterized in that: According to the delay time ΔT, the inertial navigation heading information at the time corresponding to the satellite navigation heading information is found in the cached inertial navigation data, including: Traverse the cached inertial navigation data array, compare the timestamp of each data point with the timestamp of the current satellite navigation heading information, and find the data point with the closest time difference to ΔT as the matching object.

5. The dual-antenna GNSS-assisted heading measurement method according to claim 1, characterized in that: Build a state estimation model, including: Define the system state vector X as follows, Among them, δp is the horizontal position error, δv is the horizontal velocity error, is the attitude error, δb a is the accelerometer bias, δb g is the gyro bias, δk g is the gyroscope scale coefficient error; Establish the state transfer equation as follows: X k =ψ k,k-1 X k-1 +W k ; Among them, Xk is the state vector at the current time k, X k-1 is the state vector at the previous time k-1, ψ k，k-1 is the state transfer matrix, W k is the system noise matrix; Build a measurement model, including: Define the quantity measurement Z as follows, Z = [δp δv δy]; Among them, δp is the horizontal position error, δv is the horizontal velocity error, and δy is the heading angle error; The measurement equation is established as follows: Z k =H k X k +V k ; Among them, Z k is the quantity measurement at the current time k, X k is the system state vector at the current time k, H k is the measurement matrix, V k is the measurement noise matrix.

6. A dual-antenna GNSS-assisted heading measurement method according to claim 5, characterized in that: The state estimation is filtered and iterated through the error measurement value to obtain the optimal estimate of each error term at the current moment, including: Set the initial estimate of the state vector X and initialize the covariance matrix of the state vector; Set the process noise covariance matrix and the measurement noise covariance matrix; At the beginning of each filtering cycle, the state transfer equation and the state vector estimate at the previous moment are used to predict the prior estimate of the state vector at the current moment, and at the same time, the prior estimate of the covariance matrix is ​​updated; Substitute the heading error measurement value and use the measurement equation to calculate the measurement residual; The Kalman gain is calculated based on the measurement residual, the prior estimate of the covariance matrix, and the measurement noise covariance matrix; Use the Kalman gain to update the posterior estimate of the state vector and the posterior estimate of the covariance matrix; From the updated state vector, estimates of the horizontal position error, horizontal velocity error, and heading angle error are extracted.

7. A dual-antenna GNSS-assisted heading measurement method according to claim 1, characterized in that: The validity of the heading error measurement value is judged, including: The real baseline length L1 of the dual-antenna GNSS is measured using a measuring device, and the current baseline length L2 is calculated in real time using an algorithm within the navigation system. The baseline length deviation is calculated as ΔL=L2-L1; The heading data measured by the dual-antenna GNSS is continuously recorded over a period of time, and the standard deviation σyaw of the heading data over this period of time is calculated using the standard deviation formula; Set the baseline length deviation threshold ΔL1 and the dual-antenna heading standard deviation threshold σyaw1. Before each measurement update, check whether the current baseline length deviation ΔL is less than ΔL1 and whether the dual-antenna heading standard deviation σyaw is less than σyaw1. If both conditions are met, the current heading error measurement is considered valid and the subsequent Kalman filter update step can be performed.

8. A dual-antenna GNSS-assisted heading measurement device, characterized in that: include: An inertial navigation component, the inertial component comprising a gyroscope, an accelerometer, a power module, an IF module, a satellite guidance solution module and a navigation solution module; The gyroscope is used to measure the angular velocity of the hull and provide heading angular rate information; the accelerometer is used to measure the acceleration of the hull and assist in navigation solution; the power module is used to provide a stable power supply for the inertial component; the IF module is used to realize the data communication of the inertial component; the satellite navigation solution module is used to receive and process satellite navigation signals and calculate satellite navigation heading information; the navigation solution module combines the data of the gyroscope, accelerometer and satellite navigation information to perform navigation solution and provide initial heading, position and speed information; The dual antenna assembly, including a main antenna and a slave antenna, is used to receive satellite navigation signals and calculate satellite navigation heading information through dual antenna technology.

9. A dual-antenna GNSS-assisted heading measurement system, characterized in that: include: The projection matrix calculation module is used to construct the projection matrix from the hull coordinate system to the satellite navigation dual antenna coordinate system; A delay time calculation module, used to calculate the delay time ΔT of the satellite navigation heading information relative to the inertial navigation heading information; The data cache module is used to cache the heading information measured by the inertial navigation and the corresponding timestamp into an array; The data synchronization module is used to find the inertial navigation heading information at the time corresponding to the satellite navigation heading information in the cached inertial navigation data according to the delay time ΔT during the satellite navigation heading update cycle; A coordinate conversion module is used to convert the found inertial navigation heading information from the hull coordinate system to the satellite navigation dual antenna coordinate system through a projection matrix; The error calculation module is used to compare the converted inertial navigation heading information with the satellite navigation heading information and calculate the difference between them, that is, the heading error measurement value; A validity judgment module is used to judge the validity of the heading error measurement value; The Kalman filter module is used to construct a state estimation model and a measurement model when the heading error measurement value is valid, bring in the heading error measurement value, obtain the optimal estimate of the heading attitude at the current moment through filtering iteration, and output the corrected navigation result.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, wherein the computer program implements the method steps of any one of claims 1 to 7 when executed by a processor.

Images