Three-dimensional scene positioning method and system based on deep coupling of laser intensity correlation and inertial navigation

Abstract

This invention discloses a three-dimensional scene positioning method and system that deeply couples laser intensity correlation and inertial navigation. Specifically, it involves: first, constructing a laser positioning system by setting up four base stations that emit light quantum pulses of different wavelengths, distinguishing the base stations by wavelength; then, obtaining prior position information through the strapdown inertial navigation system of the carrier; next, constructing a discrete candidate point array centered on the prior position; at each candidate position, calculating the sequence of light quantum pulses to be received based on its geometric positional relationship with the known base stations, and performing correlation calculations with the actual received sequence of the carrier, outputting the candidate position with the largest correlation value as the direct position estimation result; finally, inputting the direct position estimation and the positioning result of the strapdown inertial navigation system into a Kalman filter for combined filtering to obtain the optimal position estimate and compensate for sensor errors in the inertial measurement unit. This invention achieves high-precision positioning in three-dimensional scenes and enhances the reliability of long-term positioning and navigation.

Description

Technical Field

[0001] This invention relates to the field of navigation and positioning technology, and in particular to a three-dimensional scene positioning method and system that deeply couples laser intensity correlation and inertial navigation. Background Technology

[0002] With the development of science and technology and the arrival of the digital and intelligent era, high-precision navigation and positioning technology has become a key support for cutting-edge fields such as intelligent surveying and mapping, autonomous driving, smart cities, and drones. In these application scenarios, stringent requirements are placed on the continuity, accuracy, and robustness of positioning; traditional single navigation technologies are no longer sufficient to meet the increasingly complex application needs.

[0003] An Inertial Navigation System (INS) is a fully autonomous, passive navigation system. It uses an Inertial Measurement Unit (IMU) to sense the vehicle's angular velocity and specific force, and then integrates these parameters to obtain the vehicle's three-dimensional position, velocity, and attitude. Because it does not rely on external signals, INS possesses strong anti-interference capabilities and can operate normally in environments with limited GNSS signals, such as indoors, underwater, and underground. However, the core drawback of INS is that its navigation errors accumulate rapidly over time, leading to a divergence in long-term positioning accuracy, making it difficult to meet the requirements of long-term, high-precision navigation on its own.

[0004] To suppress INS error divergence, satellite-inertial navigation integrated systems have become the most representative solution. Global Navigation Satellite Systems (GNSS) provide all-weather, globally covered absolute position information, and their long-term stability complements the short-term high accuracy of INS. Through information fusion, GNSS / INS integrated navigation systems can both periodically correct the accumulated errors of INS using global information from GNSS and provide short-term navigation capabilities when GNSS signals are lost, thus significantly improving the overall accuracy and reliability of the system. However, GNSS itself has inherent limitations: its data update frequency is typically low (1–10Hz), making it difficult to perfectly match the motion of highly dynamic vehicles; satellite signals are relatively weak and susceptible to environmental factors such as urban canyons and electromagnetic interference; and it requires a long positioning time during cold starts or signal reacquisition. These problems limit the positioning performance and robustness of integrated navigation systems in complex environments or highly dynamic scenarios.

[0005] Therefore, there is an urgent need to develop a new global positioning technology that has high update rate, strong anti-interference capability and high-precision direct position output characteristics to replace or assist GNSS and deeply integrate with INS, thereby further improving the overall performance of the integrated navigation system. Summary of the Invention

[0006] The purpose of this invention is to provide a three-dimensional scene positioning method and system that features high update rate, strong anti-interference capability, and deep coupling of laser intensity correlation and inertial navigation, and can achieve high-precision direct position output.

[0007] The technical solution to achieve the purpose of this invention is: a three-dimensional scene localization method that couples laser intensity correlation and inertial navigation depth, comprising the following steps:

[0008] Step 1: Set up four laser positioning base stations. Split and filter the supercontinuum laser and transmit it to the four base stations. Each base station emits light quantum pulses of different wavelengths. The base stations are distinguished by wavelength.

[0009] Step 2: Based on the strapdown inertial navigation solution method, perform numerical integration calculation on the output data of the inertial measurement unit to obtain the prior information of the carrier's velocity and position;

[0010] Step 3: Construct a discrete position candidate point array centered on the prior position obtained by the strapdown inertial navigation system. Based on the geometric position relationship between each candidate position and the known base station, calculate the optical quantum pulse sequence to be received and perform correlation operation with the actual received sequence of the carrier. Output the candidate position with the largest correlation value as the direct position estimation result.

[0011] Step 4: Input the direct position estimate and the positioning result of the strapdown inertial navigation system into the Kalman filter for combined filtering to obtain the optimal position estimate and compensate for the sensor error of the inertial measurement unit.

[0012] A three-dimensional scene localization system that deeply couples laser intensity correlation and inertial navigation, the system being used to implement the aforementioned three-dimensional scene localization method that deeply couples laser intensity correlation and inertial navigation, comprising:

[0013] Four laser positioning base stations are used to emit light quantum pulses of different wavelengths, respectively;

[0014] An inertial measurement unit, mounted on the carrier, is used to output the carrier's angular velocity and acceleration data;

[0015] The strapdown inertial navigation calculation module is used to obtain the prior information of the carrier's velocity and position through numerical integration calculation based on the data output by the inertial measurement unit;

[0016] The optical signal receiving module is used to receive the optical quantum pulses emitted by the four laser positioning base stations, and distinguish them according to wavelength to obtain the actual received signal sequence;

[0017] The direct position estimation module is used to construct a discrete position candidate point array centered on the prior position obtained by the strapdown inertial navigation solution module. Based on the geometric positional relationship between each candidate position and the four laser positioning base stations, it calculates the sequence of photon pulses to be received, performs correlation operation between the calculated sequence and the actual received sequence, and outputs the candidate position with the largest correlation value as the direct position estimation result.

[0018] The Kalman filter module is used to combine and filter the direct position estimation result output by the direct position estimation module with the positioning result output by the strapdown inertial navigation solution module to obtain the optimal position estimate, and use the filtering result to compensate for the sensor error of the inertial measurement unit.

[0019] A computer device includes a memory and a processor, the memory and the processor being communicatively connected to each other, the memory storing computer instructions, and the processor executing the computer instructions to perform the laser intensity correlation and inertial navigation depth coupling three-dimensional scene localization method.

[0020] A computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps in the three-dimensional scene localization method of laser intensity correlation and inertial navigation deep coupling.

[0021] Compared with the prior art, the significant advantages of this invention are:

[0022] (1) Significantly improved positioning accuracy: This invention uses laser light intensity correlation for direct position estimation, combined with multi-point geometric relationship calculation and sequence correlation matching, to achieve centimeter-level positioning accuracy in three-dimensional scenes, which is better than the meter-level positioning of traditional GNSS, thereby effectively improving the overall output accuracy of the integrated navigation system;

[0023] (2) Enhanced anti-interference capability and robustness: Compared with GNSS satellite signals, laser signals have stronger directionality and anti-electromagnetic interference capability. This invention avoids signal crosstalk from a physical level by building four independent base stations to emit light quanta of different wavelengths, ensuring that stable and reliable absolute position information can still be provided in complex electromagnetic environments or satellite signal denial environments.

[0024] (3) Improved positioning frequency and dynamic adaptability: The emission and detection frequency of laser quantum pulses is much higher than that of GNSS update rate, which can provide high-frequency position observations for the integrated navigation system. This enables the system to track the high dynamic motion of the vehicle more closely and effectively compensate for the calculation error of INS when the vehicle is in violent maneuvering.

[0025] (4) Deep coupling and error compensation effect: The present invention integrates the direct position estimation result of the laser positioning system with the solution result of the strapdown inertial navigation system through a Kalman filter. The high-precision and high-frequency position observation input not only improves the optimal estimation accuracy of the filtered position, but also estimates and compensates for the errors of the IMU sensor zero bias and scale factor in real time and accurately, thereby suppressing the divergence rate of pure inertial navigation error from the root.

[0026] (5) Balancing positioning continuity and reliability: By constructing a candidate point array centered on the inertial navigation prior position and performing laser correlation matching, continuous high-precision positioning is achieved in the absence of GNSS signal; This method combines the short-term prediction capability of INS and the absolute correction capability of laser positioning, realizing a deep coupling mechanism of "inertial navigation short-term high-precision calculation + laser periodic high-precision calibration", ensuring the continuous and robust operation of the system under all working conditions. Attached Figure Description

[0027] Figure 1 This is a flowchart of the three-dimensional scene positioning method of the present invention, which deeply couples laser intensity correlation and inertial navigation.

[0028] Figure 2 This is a schematic diagram of the positioning system based on laser intensity correlation in an embodiment of the present invention.

[0029] Figure 3 This is a schematic diagram of the structure of the discrete position candidate point array constructed in an embodiment of the present invention.

[0030] Figure 4 This is a schematic diagram of the direct position estimation process in an embodiment of the present invention.

[0031] Figure 5 This is a schematic diagram illustrating the process of deeply coupling direct position estimation with the positioning results of the strapdown inertial navigation system in an embodiment of the present invention. Detailed Implementation

[0032] It is readily understood that, based on the technical solution of this invention, those skilled in the art can conceive of various embodiments of this invention without altering its essential spirit. Therefore, the following specific embodiments and accompanying drawings are merely illustrative examples of the technical solution of this invention and should not be considered as the entirety of this invention or as limitations or restrictions on its technical solution.

[0033] This invention provides a three-dimensional scene localization method that couples laser intensity correlation and inertial navigation depth, comprising the following steps:

[0034] Step 1: Set up four laser positioning base stations. Split and filter the supercontinuum laser and transmit it to the four base stations. Each base station emits light quantum pulses of different wavelengths. The base stations are distinguished by wavelength.

[0035] Step 2: Based on the strapdown inertial navigation solution method, perform numerical integration calculation on the output data of the inertial measurement unit to obtain the prior information of the carrier's velocity and position;

[0036] Step 3: Construct a discrete position candidate point array centered on the prior position obtained by the strapdown inertial navigation system. Based on the geometric position relationship between each candidate position and the known base station, calculate the optical quantum pulse sequence to be received and perform correlation operation with the actual received sequence of the carrier. Output the candidate position with the largest correlation value as the direct position estimation result.

[0037] Step 4: Input the direct position estimate and the positioning result of the strapdown inertial navigation system into the Kalman filter for combined filtering to obtain the optimal position estimate and compensate for the sensor error of the inertial measurement unit.

[0038] As a specific example, step 1 involves setting up four laser positioning base stations. The supercontinuum laser is split, filtered, and transmitted to the four base stations. Each base station emits light quantum pulses of different wavelengths, and the base stations are distinguished by wavelength, as detailed below:

[0039] Step 1.1: Set up four laser positioning base stations;

[0040] Step 1.2: The supercontinuum laser is separated into four wavelength channels by a beam splitter and spectral filtering, and then sent to the four base stations corresponding to each wavelength channel. The base stations then emit quantum pulse signals with a unique center wavelength.

[0041] Step 1.3: The carrier uses a photon receiver of the corresponding wavelength to receive the signal and distinguishes the base station from the processed signal sequence according to the wavelength.

[0042] As a specific example, in step 1, the frequency of the optical quantum pulse signal emitted by the base station is 2MHz; in step 1.3, the sampling frequency of the signal sequence is 5GHz.

[0043] As a specific example, the strapdown inertial navigation system-based solution method described in step 2 involves numerical integration of the output data from the inertial measurement unit to obtain the prior information on the velocity and position of the carrier, as detailed below:

[0044] Step 2.1, Incremental sampling of gyroscope angles in inertial measurement unit at any moment As input, after conical error compensation, the attitude update algorithm of the strapdown inertial navigation system is used to obtain the result. The posture of the moment Output, Indicates the first Each sampling time, express The coordinate system of the carrier at time express The navigation coordinate system at any given moment;

[0045] Step 2.2, Incremental sampling of gyroscope angles in inertial measurement unit at any moment , Accelerometer velocity increment sampling at all times , The posture of the moment As input, the paddle error of the two-sample velocity is used for compensation, and the specific force velocity increment in the navigation coordinate system is obtained by numerical integration algorithm. The harmful acceleration is approximately solved by extrapolation. ;in, Indicates the first Each sampling time, express The coordinate system of the carrier at time express The navigation coordinate system at any given moment; Indicates comparison, express The comparison of time, Indicates the navigation coordinate system; Represents Coriolis acceleration. Represents gravitational acceleration;

[0046] Step 2.3: Based on the speed update algorithm of the strapdown inertial navigation system, the recursive form of the inertial navigation specific force equation is as follows:

[0047]

[0048] In the formula, , They are , Comparison speed under real-time navigation system;

[0049] Step 2.4: Obtain the position update algorithm in discrete form. Position of the time carrier in the geodetic coordinate system The details are as follows:

[0050]

[0051] in,

[0052] ,

[0053] In the formula, express The position of the carrier in the geodetic coordinate system at any given time; This represents the middle value of the position update matrix. It is derived by extrapolation. Indicates position-velocity, subscript express and Midpoint between moments ; The calculation period; The radius of curvature of the meridian circle where the carrier is located. The radius of curvature of the zonal circle where the carrier is located; for The longitude of the carrier at that time for The radius of curvature of the zonal circle where the carrier is located at any given time. for The altitude of the carrier at any given time; for The longitude of the carrier at that time for The latitude of the carrier at any given time for The altitude of the carrier at any given time.

[0054] As a specific example, step 3 involves constructing a discrete candidate position array centered on the prior position obtained by the strapdown inertial navigation system. Based on the geometric positional relationship between each candidate position and the known base station, the sequence of optical quantum pulses to be received is calculated and correlated with the actual received sequence on the carrier. The candidate position with the largest correlation value is output as the direct position estimation result, as detailed below:

[0055] Step 3.1: Transfer the position obtained by the strapdown inertial navigation system As a priori location, a discrete array of candidate locations is constructed in three-dimensional space centered on the priori location. The interval between adjacent candidate points in the array is [missing information]. , forming a side length of A cube array;

[0056] Step 3.2: At each candidate location, based on the geometric positional relationship between the candidate location and the known laser base stations, calculate the time difference between the quantum pulses from the four base stations. Based on the time differences, construct four time lengths corresponding to the same moment. A single-pulse signal sequence of milliseconds (ms) is formed by concatenating four sequences in order of base station number to a time length. ms-based signal sequence If the base station With base station The time difference of arrival at the carrier is Then the signal sequence is deduced. The first in The pulse and the first The pulse interval is ms;

[0057] Step 3.3: Simultaneously process the sequences obtained from the four photodetectors at different wavelengths according to... The time interval is divided into blocks, ensuring that the optical pulse signals emitted by the base station at the same moment all fall within the same time block. Four sequence blocks within the time period containing the positioning time are extracted and spliced ​​together according to the base station sequence number identified by the wavelength, forming a time block with a length of [missing information]. actual received sequence in ms , The sampling point number;

[0058] Step 3.4: Calculate the signal sequence With the actual received sequence The correlation function is defined in the following discrete form:

[0059]

[0060] In the formula, A correlation function representing the actual received sequence and the estimated signal sequence; The translation amount represents the translation. One sampling point; for The number of sampling points contained within ms; It is to shift the time of the calculated signal sequence to the right, i.e., to lag it. The sequence obtained after a sampling time interval;

[0061] Step 3.5: Find the maximum value of the relevant function. The relevant values ​​for candidate positions;

[0062] Step 3.6: While traversing all candidate positions, if the correlation value of the current candidate position is greater than the correlation value of the previous candidate position, then update the stored correlation value and the position information.

[0063] Step 3.7: Output the candidate location with the highest correlation value, convert it to geodetic coordinates, and use it as the location measurement value for direct location estimation. .

[0064] As a specific example, in step 3.1, the interval between adjacent candidate points in the array , forming side length A cube array;

[0065] In step 3.2, four time lengths corresponding to the same moment are constructed based on the time difference. A single-pulse signal sequence in milliseconds (ms).

[0066] As a specific example, step 4 involves inputting the direct position estimate and the positioning result from the strapdown inertial navigation system into a Kalman filter for combined filtering to obtain the optimal position estimate, thereby compensating for the sensor error of the inertial measurement unit, as detailed below:

[0067] Step 4.1, the state-space equation of the integrated navigation system is:

[0068]

[0069] in

[0070]

[0071] , ,

[0072] In the formula, This is the state vector of the integrated navigation system. , , These are the attitude misalignment angles in the east, north, and sky directions, respectively; , , These are latitude error, longitude error, and altitude error, respectively. , , These represent the velocity errors in the east, north, and sky directions, respectively. , , These represent the random constant drift of the gyroscope along the x, y, and z axes in the carrier coordinate system, respectively. , , These represent the zero-position drift of the accelerometer along the x, y, and z axes in the carrier coordinate system, respectively. This is the system state transition matrix; , , These are the error matrix of the strapdown inertial navigation system, the error transformation matrix of the inertial device, and the error matrix of the inertial device, respectively. The system noise driving matrix, Let be the attitude matrix of the carrier system relative to the navigation coordinate system. For system noise variables, White noise for gyroscope angular velocity measurement White noise for accelerometer specific force measurement;

[0073] Step 4.2: The integrated navigation system subtracts the position measurement from the direct position estimate from the strapdown inertial navigation system, and uses this difference as the input filter for combined filtering. The observation equation is:

[0074]

[0075] in

[0076]

[0077] In the formula, For the system observation vector, For the position measurement vector of the strapdown inertial navigation system, The position measurement vector for direct position estimation; For the system observation matrix, White noise array for position measurement; The matrix represents the position differential equation of the strapdown inertial navigation system. The velocity vector at the current moment calculated by the strapdown inertial navigation system;

[0078] Step 4.3: Feed back the attitude misalignment angle, position error, and velocity error in the filter state vector to the strapdown inertial navigation system. After correcting the position and velocity parameter information, output the optimal estimation result of the integrated navigation system. Then, use the gyroscope random constant drift and accelerometer zero drift obtained from the filter to compensate for the sensor error of the inertial measurement unit.

[0079] This invention also provides a three-dimensional scene positioning system that deeply couples laser intensity correlation and inertial navigation. This system is used to implement the aforementioned three-dimensional scene positioning method that deeply couples laser intensity correlation and inertial navigation, and includes:

[0080] Four laser positioning base stations are used to emit light quantum pulses of different wavelengths, respectively;

[0081] An inertial measurement unit, mounted on the carrier, is used to output the carrier's angular velocity and acceleration data;

[0082] The strapdown inertial navigation calculation module is used to obtain the prior information of the carrier's velocity and position through numerical integration calculation based on the data output by the inertial measurement unit;

[0083] The optical signal receiving module is used to receive the optical quantum pulses emitted by the four laser positioning base stations, and distinguish them according to wavelength to obtain the actual received signal sequence;

[0084] The direct position estimation module is used to construct a discrete position candidate point array centered on the prior position obtained by the strapdown inertial navigation solution module. Based on the geometric positional relationship between each candidate position and the four laser positioning base stations, it calculates the sequence of photon pulses to be received, performs correlation operation between the calculated sequence and the actual received sequence, and outputs the candidate position with the largest correlation value as the direct position estimation result.

[0085] The Kalman filter module is used to combine and filter the direct position estimation result output by the direct position estimation module with the positioning result output by the strapdown inertial navigation solution module to obtain the optimal position estimate, and use the filtering result to compensate for the sensor error of the inertial measurement unit.

[0086] The present invention also provides a computer device, characterized in that it includes: a memory and a processor, the memory and the processor being communicatively connected to each other, the memory storing computer instructions, and the processor executing the computer instructions to perform the laser intensity correlation and inertial navigation depth coupling three-dimensional scene positioning method.

[0087] The present invention also provides a computer-readable storage medium having a computer program stored thereon, characterized in that, when the program is executed by a processor, it implements the steps in the three-dimensional scene positioning method of laser intensity correlation and inertial navigation depth coupling.

[0088] The present invention will now be described in further detail with reference to the accompanying drawings and specific embodiments.

[0089] Example

[0090] like Figure 1 As shown, the present invention provides a three-dimensional scene localization method based on laser intensity correlation and inertial navigation depth coupling, comprising the following steps:

[0091] Step 1: Set up four laser positioning base stations. Split and filter the supercontinuum laser beam and transmit it to the four base stations. Each base station emits light quantum pulses of different wavelengths. The base stations are distinguished by wavelength, as detailed below:

[0092] Step 1.1: Set up four laser positioning base stations, such as... Figure 2 As shown;

[0093] Step 1.2: The supercontinuum laser is split into four wavelength channels by a beam splitter and spectral filtering, and then sent to the four base stations corresponding to each wavelength channel. The base stations emit quantum pulse signals with a unique center wavelength and a pulse signal frequency of 2MHz. By calibrating the signal time through the optical transmission time between the laser source and the base station obtained in advance, it can be assumed that the optical pulse signals of the base stations are emitted at the same time.

[0094] Step 1.3: The carrier uses a photon receiver of the corresponding wavelength to receive the signal and distinguishes the base station from the processed signal sequence according to the wavelength. The sampling frequency of the sequence is 5 GHz, which can meet the centimeter-level positioning requirements.

[0095] Step 2: Based on the strapdown inertial navigation system (SINS) calculation method, perform numerical integration calculations on the output data of the inertial measurement unit (INS) to obtain the prior information of the carrier's velocity and position, as detailed below:

[0096] Step 2.1: Sample the incremental gyroscope angle of the inertial measurement unit. As input, after conical error compensation, the attitude matrix is ​​calculated by the attitude update algorithm of the strapdown inertial navigation system. Output;

[0097] Step 2.2, Incremental sampling of gyroscope angles in inertial measurement unit at any moment , Accelerometer velocity increment sampling at all times , The posture of the moment As input, the paddle error of the two-sample velocity is used for compensation, and the specific force velocity increment in the navigation coordinate system is obtained by numerical integration algorithm. The harmful acceleration is approximately solved by extrapolation. ;in, Indicates the first Each sampling time, express The coordinate system of the carrier at time express The navigation coordinate system at any given moment; Indicates comparison, express The comparison of time, Indicates the navigation coordinate system; Represents Coriolis acceleration. Represents gravitational acceleration;

[0098] Step 2.3: Based on the speed update algorithm of the strapdown inertial navigation system, the recursive form of the inertial navigation specific force equation is as follows:

[0099]

[0100] In the formula, , They are , The speed comparison under the navigation system at all times;

[0101] Step 2.4: From the discrete form of the position update algorithm, we can obtain... Position of the carrier in the geodetic coordinate system at any moment The details are as follows:

[0102]

[0103] in,

[0104] ,

[0105] In the formula, express The position of the carrier in the geodetic coordinate system at any given time; This represents the middle value of the position update matrix. It is derived by extrapolation. Indicates position-velocity, subscript express and Midpoint between moments ; The calculation period; The radius of curvature of the meridian circle where the carrier is located. The radius of curvature of the zonal circle where the carrier is located; for The longitude of the carrier at that time for The radius of curvature of the zonal circle where the carrier is located at any given time. for The altitude of the carrier at any given time; for The longitude of the carrier at that time for The latitude of the carrier at any given time for The altitude of the carrier at any given time.

[0106] Step 3: Construct a discrete candidate position array centered on the prior position obtained by strapdown inertial navigation. Based on the geometric positional relationship between each candidate position and the known base station, calculate the quantum pulse sequence to be received and perform correlation calculation with the actual received sequence on the carrier. Output the candidate position with the largest correlation value as the direct position estimation result, as follows:

[0107] Step 3.1: Obtain position from strapdown inertial navigation system Using this as a priori location, a discrete array of candidate locations is constructed in three-dimensional space, such as... Figure 3 As shown, the interval between adjacent candidate points in the array is 5cm, forming a cube array with a side length of 1m;

[0108] Step 3.2: At each candidate location, based on its geometric positional relationship with the laser base stations at known locations, calculate the time difference between the quantum pulses from the four base stations. Based on the time difference, construct four single-pulse signal sequences with a duration of 1ms each, corresponding to the same moment. Concatenate the four sequences according to the base station number to form a calculated signal sequence with a duration of 4ms. If the base station With base station The time difference of arrival at the carrier is Then the signal sequence is deduced. The first in The pulse and the first The pulse interval is ms;

[0109] Step 3.3: Simultaneously divide the sequences obtained from the four photodetectors of different wavelengths into blocks at 1ms time intervals, ensuring that the optical pulse signals emitted by the base station at the same time all fall within the same time block, such as... Figure 4 As shown, four sequence blocks within the time period of the positioning time are extracted and concatenated according to the base station sequence number identified by the wavelength to form an actual received sequence with a time length of 1ms. ;

[0110] Step 3.4: Calculate the signal sequence With the actual received sequence Perform correlation calculations, where the correlation function can be defined in the following discrete form:

[0111]

[0112] In the formula, A correlation function representing the actual received sequence and the estimated signal sequence; The translation amount represents the translation. One sampling point; This represents the number of sampling points contained within 1 ms. It is to shift the time of the calculated signal sequence to the right, i.e., to lag it. The sequence obtained after a sampling time interval;

[0113] Step 3.5: Find the maximum value of the relevant function. This serves as the relevant value result for this candidate position;

[0114] Step 3.6: While traversing all candidate positions, if the correlation value of this candidate position is greater than the correlation value of the previous candidate position, then update the stored correlation value and the position information.

[0115] Step 3.7: Output the candidate location with the highest correlation value, convert it to geodetic coordinates, and use it as the location measurement value for direct location estimation. .

[0116] Step 4: Input the direct position estimate and the positioning result from the strapdown inertial navigation system into a Kalman filter for combined filtering to obtain the optimal position estimate. Based on this estimate, compensate for the sensor error of the inertial measurement unit, such as... Figure 5 As shown, the details are as follows:

[0117] Step 4.1: Estimate the error in the system using an Error-State Kalman Filter (ESKF), and feed the estimated value back to the system as a correction value to achieve integrated correction; the lever error between the photodetector and the inertial measurement unit is accurately measured and compensated beforehand, therefore it is not included in the filter state; the state space equation of the integrated navigation system is:

[0118]

[0119] in

[0120]

[0121] , ,

[0122] In the formula, This is the state vector of the integrated navigation system. , , These are the attitude misalignment angles in the east, north, and sky directions, respectively; , , These are latitude error, longitude error, and altitude error, respectively. , , These represent the velocity errors in the east, north, and sky directions, respectively. , , These represent the random constant drift of the gyroscope along the x, y, and z axes in the carrier coordinate system, respectively. , , These represent the zero-position drift of the accelerometer along the x, y, and z axes in the carrier coordinate system, respectively. This is the system state transition matrix; , , These are the error matrix of the strapdown inertial navigation system, the error transformation matrix of the inertial device, and the error matrix of the inertial device, respectively. The system noise driving matrix, Let be the attitude matrix of the carrier system relative to the navigation coordinate system. For system noise variables, White noise for gyroscope angular velocity measurement White noise for accelerometer specific force measurement;

[0123] Step 4.2: The integrated navigation system subtracts the position measurement from the direct position estimate from the strapdown inertial navigation system, and uses this difference as the input filter for combined filtering. The observation equation is as follows:

[0124]

[0125] in

[0126]

[0127] In the formula, For the system observation vector, For the position measurement vector of the strapdown inertial navigation system, The position measurement vector for direct position estimation; For the system observation matrix, White noise array for position measurement; The matrix represents the position differential equation of the strapdown inertial navigation system. The velocity vector at the current moment calculated by the strapdown inertial navigation system;

[0128] Step 4.3: Feed back the attitude misalignment angle, position error, and velocity error in the filter state vector to the strapdown inertial navigation system, correct its position and velocity parameter information, and output it as the optimal estimation result of the integrated navigation system. Then, use the gyroscope random constant drift and accelerometer zero drift obtained from the filter to compensate for the sensor error of the inertial measurement unit.

[0129] This embodiment utilizes a strapdown inertial navigation system (SINS) to obtain prior position information, and constructs a discrete candidate point array centered on this information. At each candidate position, based on its geometric positional relationship with the known optical pulse transmitting base station, the sequence to be received is calculated and correlated with the actual received sequence to obtain a direct position estimation result. This result is then combined and filtered with the SINS positioning result to obtain the optimal position estimate and compensate for sensor errors in the inertial measurement unit, achieving high-precision positioning in a 3D scene. Furthermore, combining the advantages of laser intensity correlation positioning and inertial navigation positioning, based on the direct position estimation method, it uses the prior information obtained from the SINS for absolute positioning and corrects the SINS results, achieving deep coupling between laser intensity correlation absolute positioning and strapdown inertial navigation relative positioning, thus enhancing the accuracy and reliability of long-term positioning and navigation.

[0130] The above are merely preferred embodiments of the present invention. It should be noted that those skilled in the art can make various improvements and modifications without departing from the principle of the present invention, and these improvements and modifications should also be considered within the scope of protection of the present invention.

Claims

1. A three-dimensional scene localization method that deeply couples laser intensity correlation and inertial navigation, characterized in that, Includes the following steps: Step 1: Set up four laser positioning base stations. Split and filter the supercontinuum laser and transmit it to the four base stations. Each base station emits light quantum pulses of different wavelengths. The base stations are distinguished by wavelength. Step 2: Based on the strapdown inertial navigation solution method, perform numerical integration calculation on the output data of the inertial measurement unit to obtain the prior information of the carrier's velocity and position; Step 3: Construct a discrete candidate position array centered on the prior position obtained by the strapdown inertial navigation system. Based on the geometric positional relationship between each candidate position and the known base station, calculate the quantum pulse sequence to be received and perform correlation calculation with the actual received sequence on the carrier. Output the candidate position with the largest correlation value as the direct position estimation result, as follows: Step 3.1: Transfer the position obtained by the strapdown inertial navigation system As a priori location, a discrete array of candidate locations is constructed in three-dimensional space centered on the priori location. The interval between adjacent candidate points in the array is [missing information]. , forming a side length of A cube array; Step 3.2: At each candidate location, based on the geometric positional relationship between the candidate location and the known laser base stations, calculate the time difference between the quantum pulses from the four base stations. Based on the time differences, construct four time lengths corresponding to the same moment. A single-pulse signal sequence of milliseconds (ms) is formed by concatenating four sequences in order of base station number to a time length. ms-based signal sequence ; If base station With base station The time difference of arrival at the carrier is Then the signal sequence is deduced. The first in The pulse and the first The pulse interval is ms; Step 3.3: Simultaneously process the sequences obtained from the four photodetectors at different wavelengths according to... The time interval is divided into blocks, ensuring that the optical pulse signals emitted by the base station at the same moment all fall within the same time block. Four sequence blocks within the time period containing the positioning time are extracted and concatenated according to the base station sequence number identified by the wavelength, forming a time block with a length of [missing information]. actual received sequence in ms , The sampling point number; Step 3.4: Calculate the signal sequence With the actual received sequence The correlation function is defined in the following discrete form: In the formula, A correlation function representing the actual received sequence and the estimated signal sequence; The translation amount represents the translation. One sampling point; for The number of sampling points contained within ms; It is to shift the time of the calculated signal sequence to the right, i.e., to lag it. The sequence obtained after a sampling time interval; Step 3.5: Find the maximum value of the relevant function. The relevant values ​​for candidate positions; Step 3.6: While traversing all candidate positions, if the correlation value of the current candidate position is greater than the correlation value of the previous candidate position, then update the stored correlation value and the position information. Step 3.7: Output the candidate location with the highest correlation value, convert it to geodetic coordinates, and use it as the location measurement value for direct location estimation. ; Step 4: Input the direct position estimate and the positioning result of the strapdown inertial navigation system into the Kalman filter for combined filtering to obtain the optimal position estimate and compensate for the sensor error of the inertial measurement unit.

2. The three-dimensional scene localization method based on laser intensity correlation and inertial navigation depth coupling according to claim 1, characterized in that, Step 1 involves setting up four laser positioning base stations. The supercontinuum laser is split, filtered, and transmitted to the four base stations. Each base station emits a quantum pulse of a different wavelength, and the base stations are distinguished by wavelength. The details are as follows: Step 1.1: Set up four laser positioning base stations; Step 1.2: The supercontinuum laser is separated into four wavelength channels by a beam splitter and spectral filtering, and then sent to the four base stations corresponding to each wavelength channel. The base stations then emit quantum pulse signals with a unique center wavelength. Step 1.3: The carrier uses a photon receiver of the corresponding wavelength to receive the signal and distinguishes the base station from the processed signal sequence according to the wavelength.

3. The three-dimensional scene positioning method based on laser intensity correlation and inertial navigation depth coupling according to claim 2, characterized in that, In step 1, the frequency of the optical quantum pulse signal emitted by the base station is 2MHz; in step 1.3, the sampling frequency of the signal sequence is 5GHz.

4. The three-dimensional scene localization method based on laser intensity correlation and inertial navigation depth coupling according to claim 1, characterized in that, Step 2 describes a strapdown inertial navigation system-based calculation method that performs numerical integration on the output data of the inertial measurement unit to obtain the prior information on the velocity and position of the carrier, as detailed below: Step 2.1, Incremental sampling of gyroscope angles in inertial measurement unit at any moment As input, after conical error compensation, the attitude update algorithm of the strapdown inertial navigation system is used to obtain the result. The posture of the moment Output, Indicates the first Each sampling time, express The coordinate system of the carrier at time express The navigation coordinate system at any given moment; Step 2.2, Incremental sampling of gyroscope angles in inertial measurement unit at any moment , Accelerometer velocity increment sampling at all times , The posture of the moment As input, the paddle error of the two-sample velocity is used for compensation, and the specific force velocity increment in the navigation coordinate system is obtained by numerical integration algorithm. The harmful acceleration is approximately solved by extrapolation. ;in, Indicates the first Each sampling time, express The coordinate system of the carrier at time express The navigation coordinate system at any given moment; Indicates comparison, express The comparison of time, Indicates the navigation coordinate system; Represents Coriolis acceleration. Represents gravitational acceleration; Step 2.3: Based on the speed update algorithm of the strapdown inertial navigation system, the recursive form of the inertial navigation specific force equation is as follows: In the formula, , They are , Comparison speed under real-time navigation system; Step 2.4: Obtain the position update algorithm in discrete form. Position of the time carrier in the geodetic coordinate system The details are as follows: in, ， In the formula, express The position of the carrier in the geodetic coordinate system at any given time; This represents the middle value of the position update matrix. It is derived by extrapolation. Indicates position-velocity, subscript express and Midpoint between moments ; The calculation period; The radius of curvature of the meridian circle where the carrier is located. The radius of curvature of the zonal circle where the carrier is located; for The longitude of the carrier at that time for The radius of curvature of the zonal circle where the carrier is located at any given time. for The altitude of the carrier at any given time; for The longitude of the carrier at that time for The latitude of the carrier at any given time for The altitude of the carrier at any given time.

5. The three-dimensional scene localization method based on laser intensity correlation and inertial navigation depth coupling according to claim 4, characterized in that, In step 3.1, the interval between adjacent candidate points in the array , forming side length A cube array; In step 3.2, four time lengths corresponding to the same moment are constructed based on the time difference. A single-pulse signal sequence in milliseconds (ms).

6. The three-dimensional scene localization method based on laser intensity correlation and inertial navigation depth coupling according to claim 5, characterized in that, Step 4 involves inputting the direct position estimate and the positioning result from the strapdown inertial navigation system into a Kalman filter for combined filtering to obtain the optimal position estimate, thereby compensating for the sensor error of the inertial measurement unit. The specific steps are as follows: Step 4.1, the state-space equation of the integrated navigation system is: in ， ， In the formula, This is the state vector of the integrated navigation system. , , These are the attitude misalignment angles in the east, north, and sky directions, respectively; , , These are latitude error, longitude error, and altitude error, respectively. , , These represent the velocity errors in the east, north, and sky directions, respectively. , , These represent the random constant drift of the gyroscope along the x, y, and z axes in the carrier coordinate system, respectively. , , These represent the zero-position drift of the accelerometer along the x, y, and z axes in the carrier coordinate system, respectively. This is the system state transition matrix; , , These are the error matrix of the strapdown inertial navigation system, the error transformation matrix of the inertial device, and the error matrix of the inertial device, respectively. The system noise driving matrix, Let be the attitude matrix of the carrier system relative to the navigation coordinate system. For system noise variables, White noise for gyroscope angular velocity measurement White noise for accelerometer specific force measurement; Step 4.2: The integrated navigation system subtracts the position measurement from the direct position estimate from the strapdown inertial navigation system, and uses this difference as the input filter for combined filtering. The observation equation is: in In the formula, For the system observation vector, For the position measurement vector of the strapdown inertial navigation system, The position measurement vector for direct position estimation; For the system observation matrix, White noise array for position measurement; The matrix represents the position differential equation of the strapdown inertial navigation system. The velocity vector at the current moment calculated by the strapdown inertial navigation system; Step 4.3: Feed back the attitude misalignment angle, position error, and velocity error in the filter state vector to the strapdown inertial navigation system. After correcting the position and velocity parameter information, output the optimal estimation result of the integrated navigation system. Then, use the gyroscope random constant drift and accelerometer zero drift obtained from the filter to compensate for the sensor error of the inertial measurement unit.

7. A three-dimensional scene positioning system that deeply couples laser intensity correlation and inertial navigation, characterized in that, This system is used to implement the three-dimensional scene localization method of laser intensity correlation and inertial navigation depth coupling as described in any one of claims 1 to 6, comprising: Four laser positioning base stations are used to emit light quantum pulses of different wavelengths, respectively; An inertial measurement unit, mounted on the carrier, is used to output the carrier's angular velocity and acceleration data; The strapdown inertial navigation calculation module is used to obtain the prior information of the carrier's velocity and position through numerical integration calculation based on the data output by the inertial measurement unit; The optical signal receiving module is used to receive the optical quantum pulses emitted by the four laser positioning base stations, and distinguish them according to wavelength to obtain the actual received signal sequence; The direct position estimation module is used to construct a discrete position candidate point array centered on the prior position obtained by the strapdown inertial navigation solution module. Based on the geometric positional relationship between each candidate position and the four laser positioning base stations, it calculates the sequence of photon pulses to be received, performs correlation operation between the calculated sequence and the actual received sequence, and outputs the candidate position with the largest correlation value as the direct position estimation result. The Kalman filter module is used to combine and filter the direct position estimation result output by the direct position estimation module with the positioning result output by the strapdown inertial navigation solution module to obtain the optimal position estimate, and use the filtering result to compensate for the sensor error of the inertial measurement unit.

8. A computer device, characterized in that, include: The system includes a memory and a processor, which are interconnected. The memory stores computer instructions, and the processor executes the computer instructions to perform the three-dimensional scene localization method of laser intensity correlation and inertial navigation depth coupling as described in any one of claims 1 to 6.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the program is executed by the processor, it implements the steps in the three-dimensional scene localization method of laser intensity correlation and inertial navigation deep coupling as described in any one of claims 1 to 6.

Images