Laser light intensity correlation and inertial navigation depth coupled three-dimensional scene positioning method and system

Abstract

The invention discloses a three-dimensional scene positioning method and system for laser light intensity correlation and inertial navigation depth coupling, and the method specifically comprises the steps: firstly constructing a laser positioning system, building four base stations, respectively transmitting light quantum pulses with different wavelengths, and distinguishing the base stations through the wavelengths; then obtaining prior position information through a strapdown inertial navigation system of a carrier; a discrete position candidate point array is constructed with the prior position as the center, a light quantum pulse sequence needing to be received is calculated at each candidate position according to the geometric position relation between the candidate position and a known base station, correlation operation is carried out on the light quantum pulse sequence and an actual receiving sequence of a carrier, and the candidate position with the maximum correlation value is output to serve as a direct position estimation result; and 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 estimation of the position, and compensating the sensor error of the inertial measurement unit. According to the invention, high-precision positioning of a three-dimensional scene is realized, and the reliability of long-time positioning navigation is enhanced.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of navigation positioning, in particular to a three-dimensional scene positioning method and system based on deep coupling of laser intensity correlation and inertial navigation. BACKGROUND

[0002] With the development of science and technology and the advent of the digital and intelligent era, high-precision navigation and positioning technology has become a key support for frontiers such as intelligent mapping, unmanned driving, smart cities, and unmanned aerial vehicles. In the above application scenarios, strict requirements are put forward for the continuity, precision, and robustness of positioning, and traditional single navigation technology has been difficult to meet the increasingly complex application requirements.

[0003] An inertial navigation system (INS) is a completely autonomous passive navigation system. It perceives the angular velocity and specific force information of the carrier itself through an inertial measurement unit (IMU), and obtains the three-dimensional position, velocity, and attitude of the carrier through integration. Since it does not rely on external signals, INS has strong anti-interference ability and can work normally in environments where GNSS signals are limited, such as indoors, underwater, and underground. However, the core defect of INS is that its navigation error will quickly accumulate over time, leading to long-term positioning accuracy divergence, making it difficult to meet the long-term and high-precision navigation requirements alone.

[0004] In order to suppress the error divergence of INS, satellite-inertial combined navigation system has become the most representative solution. Global Navigation Satellite System (GNSS) can provide all-weather, global coverage of absolute position information, and its long-term stability and short-term high precision of INS are complementary. Through information fusion, GNSS / INS combined navigation system can not only use the global information of GNSS to correct the accumulated error of INS periodically, but also provide short-time navigation capability when GNSS signal is lost, thereby significantly improving the overall accuracy and reliability of the system. However, GNSS itself has inherent limitations: its data update frequency is usually low (1-10 Hz), which is difficult to completely match the motion of high dynamic carriers; satellite signals are relatively weak and are easily affected by urban canyons, electromagnetic interference, and other environments; and it takes a long time to position when cold starting or signal recapturing. These problems limit the positioning performance and robustness of the combined navigation system in complex environments or high dynamic scenarios.

[0005] Therefore, it is urgent to develop a new global positioning technology, which should have high update rate, strong anti-interference ability and high-precision direct position output characteristics to replace or assist GNSS and deeply integrate with INS, so as to further improve the overall performance of the integrated navigation system. SUMMARY

[0006] The application aims to provide a laser light intensity correlation and inertial navigation deeply coupled three-dimensional scene positioning method and system with high update rate and strong anti-interference ability, and capable of realizing high-precision direct position output.

[0007] The technical solution for achieving the application is as follows: a laser light intensity correlation and inertial navigation deeply coupled three-dimensional scene positioning method, comprising the following steps:

[0008] Step 1: Four laser positioning base stations are built, and super-continuous laser is split, filtered and transmitted to the four base stations, each of which emits light quantum pulses of different wavelengths to distinguish the base stations by wavelength;

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

[0010] Step 3: A discrete position candidate point array is constructed with the prior position obtained by the strapdown inertial navigation system as the center, the light quantum pulse sequence that should be received is calculated according to the geometric position relationship between each candidate position and the known base station, and the correlation operation is performed with the actual received sequence to output the candidate position with the maximum correlation value as the direct position estimation result;

[0011] Step 4: The direct position estimation and the positioning result of the strapdown inertial navigation system are input into a Kalman filter for combined filtering to obtain the optimal estimation of the position and compensate for the sensor errors of the inertial measurement unit.

[0012] A laser light intensity correlation and inertial navigation deeply coupled three-dimensional scene positioning system, which is used to realize the laser light intensity correlation and inertial navigation deeply coupled three-dimensional scene positioning method, comprises:

[0013] Four laser positioning base stations for respectively emitting light quantum pulses of different wavelengths;

[0014] An inertial measurement unit arranged on the carrier for outputting the angular velocity and acceleration data of the carrier;

[0015] A strapdown inertial navigation solution module for obtaining the velocity and position prior information of the carrier through numerical integration solution according to the data output by the inertial measurement unit;

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

[0017] The direct position estimation module is configured to construct an array of discrete position candidate points with the prior position obtained by the SINS module as a center, calculate a sequence of optical quantum pulses that should be received according to geometric position relationships between each candidate position and the four laser positioning base stations, and perform correlation operation on the calculated sequence and the actual received sequence to output a candidate position with the largest correlation value as a direct position estimation result.

[0018] The Kalman filter module is configured to perform combined filtering on the direct position estimation result output by the direct position estimation module and the positioning result output by the SINS module to obtain an optimal estimation of the position, and compensate for sensor errors of the inertial measurement unit by using the filtered result.

[0019] A computer device includes a memory and a processor in communication connection with each other, the memory stores computer instructions, and the processor executes the computer instructions to implement the laser light intensity correlation and inertial navigation deep coupling three-dimensional scene positioning method.

[0020] A computer readable storage medium stores a computer program, and the program is executed by a processor to implement the steps in the laser light intensity correlation and inertial navigation deep coupling three-dimensional scene positioning method.

[0021] Compared with the prior art, the present application has the following advantages:

[0022] (1) The positioning accuracy is significantly improved: the present application uses laser light intensity correlation for direct position estimation, combines multi-point geometric relationship calculation and sequence correlation matching, and can obtain centimeter-level positioning accuracy in a three-dimensional scene, which is better than the meter-level positioning of the traditional GNSS, thereby effectively improving the overall output accuracy of the integrated navigation system.

[0023] (2) The anti-interference ability and robustness are enhanced: laser signals have stronger directivity and anti-electromagnetic interference ability than GNSS satellite signals, and the present application avoids signal crosstalk from the physical layer by building four independent base stations to emit optical quantum of different wavelengths, thereby ensuring that stable and reliable absolute position information can be provided in a complex electromagnetic environment or a satellite signal denial environment.

[0024] (3) The positioning frequency and dynamic adaptability are improved: the emission and detection frequency of laser optical quantum pulses is much higher than the GNSS update rate, which can provide high-frequency position observation values for the integrated navigation system, so that the system can more closely track the high dynamic motion of the carrier and effectively compensate for the calculation errors of the INS when the carrier is in a violent maneuver.

[0025] (4) Deep coupling and error compensation effect is better: the direct position estimation result of the laser positioning system and the calculation result of the strapdown inertial navigation system are deeply fused through the Kalman filter, high-precision and high-frequency position observation input, which not only improves the optimal estimation precision of the filtered position, but also can estimate and compensate the sensor zero offset, scale factor and other errors of the IMU in real time and accurately, thereby inhibiting the divergence speed of the pure inertial navigation error from the root;

[0026] (5) Positioning continuity and reliability are considered: laser correlation matching is realized by constructing a candidate point array centered on the inertial navigation prior position, realizing continuous high-precision positioning in the absence of GNSS signals; the method combines the short-term prediction ability of the INS and the absolute correction ability of the laser positioning, realizes the deep coupling mechanism of "inertial navigation short-time high-precision calculation + laser periodic high-precision calibration", and ensures the continuous and stable operation of the system in all working conditions. BRIEF DESCRIPTION OF DRAWINGS

[0027] Figure 1 is the flowchart of the three-dimensional scene positioning method of the laser light intensity correlation and inertial navigation deep coupling of the present application.

[0028] Figure 2 is the structure diagram of the positioning system based on laser light intensity correlation in the embodiment of the present application.

[0029] Figure 3 is the structure diagram of the discrete position candidate point array constructed in the embodiment of the present application.

[0030] Figure 4 is the flowchart of the direct position estimation in the embodiment of the present application.

[0031] Figure 5 is the flowchart of the deep coupling of the direct position estimation and the positioning result of the strapdown inertial navigation system in the embodiment of the present application. DETAILED DESCRIPTION

[0032] It is easy to understand that, according to the technical scheme of the present application, a person skilled in the art can imagine various embodiments of the present application without changing the essential spirit of the present application. Therefore, the following specific embodiments and drawings are only exemplary descriptions of the technical scheme of the present application, and should not be regarded as the whole or as a limitation or restriction of the technical scheme of the present application.

[0033] The three-dimensional scene positioning method of the laser light intensity correlation and inertial navigation deep coupling of the present application comprises the following steps:

[0034] Step 1, build four laser positioning base stations, split the super-continuous laser, filter and transmit to the four base stations, each base station emits light quantum pulse of different wavelength, and the base stations are distinguished by wavelength;

[0035] Step 2, based on the strapdown inertial navigation solution method, the output data of the inertial measurement unit is numerically integrated to obtain the velocity and position prior information of the carrier;

[0036] Step 3, the prior position obtained by the strapdown inertial navigation system is used as the center to construct a discrete position candidate point array, according to the geometric position relationship between each candidate position and the known base station, the light quantum pulse sequence to be received is calculated and correlated with the actual received sequence, and the candidate position with the maximum correlation value is output as the direct position estimation result;

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

[0038] As a specific example, step 1 builds four laser positioning base stations, splits the super-continuous laser, filters and transmits to the four base stations, each base station emits light quantum pulse of different wavelength, and the base stations are distinguished by wavelength, as follows:

[0039] Step 1.1, build four laser positioning base stations;

[0040] Step 1.2, the super-continuous laser is separated into four wavelength channels of light signals by a beam splitter and spectral filtering, and sent to the corresponding four base stations of the wavelength channels respectively, and the base stations emit light quantum pulse signals with unique center wavelength;

[0041] Step 1.3, the carrier uses a corresponding wavelength photon receiver to receive the signal, and distinguishes the base stations and processes the signal sequence according to the wavelength.

[0042] As a specific example, in step 1, the frequency of the light 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, step 2, based on the strapdown inertial navigation solution method, the output data of the inertial measurement unit is numerically integrated to obtain the velocity and position prior information of the carrier, as follows:

[0044] Step 2.1, the inertial measurement unit gyro angle increment sample at time t is As input, after conical error compensation, the attitude update algorithm of strapdown inertial navigation is used to obtain the attitude matrix at time t Output, denotes the sampled time, denotes the carrier coordinate system at the sampled time, denotes the navigation coordinate system at the sampled time;

[0045] Step 2.2, sampling the gyro angle increment of the inertial measurement unit at the sampled time, , sampling the accelerometer velocity increment at the sampled time, , the attitude matrix at the sampled time is taken as input, and a two-subsample velocity paddling error is compensated for, and the specific force velocity increment in the navigation coordinate system is obtained by a numerical integration algorithm ; wherein, denotes the sampled time, denotes the carrier coordinate system at the sampled time, denotes the navigation coordinate system at the sampled time; denotes the specific force, denotes the specific force at the sampled time, denotes the navigation coordinate system; denotes the Coriolis acceleration, denotes the gravitational acceleration;

[0046] Step 2.3, according to the velocity update algorithm of the strapdown inertial navigation, the recursive form of the inertial navigation specific force equation is:

[0047]

[0048] In the formula, , are the specific force velocities in the navigation system at the , sampled times, respectively;

[0049] Step 2.4, the position of the carrier in the geodetic coordinate system at the sampled time is obtained from the discrete form of the position update algorithm, as follows:

[0050]

[0051] wherein,

[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 moment. 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 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;

[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, according to the time difference, a sequence of four single pulse signals with time length of ms corresponding to the same moment is constructed.

[0066] As a specific example, the step 4 described that the direct position estimation is combined with the positioning result of the strapdown inertial navigation system to input the Kalman filter to filter and obtain the optimal estimation of the position, and the sensor error of the inertial measurement unit is compensated, which is as follows:

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

[0068]

[0069] Wherein

[0070]

[0071] , ,

[0072] In the formula, is the state vector of the integrated navigation system, , , are the attitude misalignment angles in the east, north and sky directions respectively; , , are the latitude error, longitude error and height error respectively; , , are the velocity errors in the east, north and sky directions respectively; , , are the random constant drifts of the gyro in the x, y and z axes in the carrier coordinate system respectively; , , are the zero position drifts of the accelerometer in the x, y and z axes in the carrier coordinate system respectively; is the system state transition matrix; , , are the strapdown inertial navigation system error matrix, inertial device error conversion matrix and inertial device error matrix respectively; is the system noise driven matrix, is the attitude matrix of the carrier relative to the navigation coordinate system, is the system noise variable, is the gyro angular velocity measurement white noise, is the accelerometer specific force measurement white noise;

[0073] Step 4.2, the integrated navigation system differentiates the position measurement value of the direct position estimation from the position measurement value of the strapdown inertial navigation system, and inputs the difference value as an observation into the filter for integrated filtering, and the observation equation is:

[0074]

[0075] Wherein

[0076]

[0077] In the formula, is a system observation vector, is a position measurement vector of the strapdown inertial navigation system, is a position measurement vector of the direct position estimation; is a system observation matrix, is a position measurement white noise matrix; is a matrix of the strapdown inertial position differential equation, is a velocity vector of the strapdown inertial navigation system at the current time;

[0078] Step 4.3, the attitude misalignment angle, the position error and the velocity error in the filter state vector are fed back to the strapdown inertial navigation system, the position and velocity parameter information after correction is output as the optimal estimation result of the integrated navigation system, and the sensor error of the inertial measurement unit is compensated by using the gyro random constant drift and the accelerometer zero drift obtained by the filter.

[0079] The application further provides a three-dimensional scene positioning system with deep coupling of laser light intensity correlation and inertial navigation, which is used for realizing the three-dimensional scene positioning method with deep coupling of laser light intensity correlation and inertial navigation, and comprises:

[0080] Four laser positioning base stations are used for respectively emitting light quantum pulses with different wavelengths.

[0081] An inertial measurement unit is arranged on the carrier and is used for outputting angular velocity and acceleration data of the carrier.

[0082] A strapdown inertial navigation calculation module is used for obtaining velocity and position prior information of the carrier through numerical integral calculation according to the data output by the inertial measurement unit.

[0083] A light signal receiving module is used for receiving the light quantum pulses emitted by the four laser positioning base stations, distinguishing the light quantum pulses according to wavelengths, and obtaining an actual received signal sequence.

[0084] A direct position estimation module is configured to construct an array of discrete position candidate points centered on the prior position obtained by the SINS module, calculate the sequence of quantum pulses that should be received according to the geometric position relationship between each candidate position and the four laser positioning base stations, and perform correlation operation on the calculated sequence and the actually received sequence, and output the candidate position with the maximum correlation value as the direct position estimation result.

[0085] A Kalman filter module is configured to perform combined filtering on the direct position estimation result output by the direct position estimation module and the positioning result output by the SINS module, obtain the optimal estimation of the position, and compensate for the sensor error of the inertial measurement unit by using the filtering result.

[0086] The application further provides a computer device, characterized by comprising a memory and a processor, which are in communication connection with each other, and the memory stores computer instructions, and the processor executes the computer instructions to perform the laser intensity correlation and inertial navigation deep coupling three-dimensional scene positioning method.

[0087] The application further provides a computer readable storage medium, which stores a computer program, and the program is executed by a processor to implement the steps in the laser intensity correlation and inertial navigation deep coupling three-dimensional scene positioning method.

[0088] The application will be further described in detail below with reference to the drawings and specific embodiments.

[0089] Embodiment

[0090] As shown in the drawings, Figure 1 The laser intensity correlation and inertial navigation deep coupling three-dimensional scene positioning method comprises the following steps:

[0091] Step 1, four laser positioning base stations are built, supercontinuum laser is split and filtered and transmitted to the four base stations, and each base station emits quantum pulse of different wavelength, and the base stations are distinguished by wavelength, specifically as follows:

[0092] Step 1.1, four laser positioning base stations are built, as shown in the drawings, Figure 2

[0093] Step 1.2, the supercontinuum laser is separated into four wavelength channel optical signals by a beam splitter and a spectral filter, and is sent to the four base stations corresponding to the wavelength channels respectively, and the base stations emit quantum pulse signals with unique center wavelength, and the pulse signal frequency is 2MHz; by pre-obtaining the optical transmission time calibration signal time between the laser source and the base station, it can be considered that the optical pulse signals of the base stations are emitted at the same time.

[0094] ​Step 1.3, the carrier utilizes a corresponding wavelength photonic receiver to receive signals, and distinguishes the base station from the processing signal sequence according to the wavelength, and the sampling frequency of the sequence is 5GHz, which can meet the positioning requirement of centimeter level.

[0095] Step 2, based on the strapdown inertial navigation solution method, the inertial measurement unit output data is numerically integrated to obtain the velocity and position prior information of the carrier, which is as follows:

[0096] Step 2.1, the inertial measurement unit gyro angle increment sampling As input, the attitude matrix is obtained by the attitude update algorithm of strapdown inertial navigation after conical error compensation

[0097] Step 2.2, the time inertial measurement unit gyro angle increment sampling , time accelerometer velocity increment sampling , time attitude matrix is taken as input, and the specific force velocity increment in the navigation coordinate system is obtained by the numerical integration algorithm , the harmful acceleration is approximately solved by extrapolation method ; wherein, represents the th sampling time, represents the time carrier coordinate system, represents the time navigation coordinate system; represents the specific force, represents the time specific force, represents the navigation coordinate system; represents the Coriolis acceleration, represents the gravity acceleration;

[0098] Step 2.3, according to the velocity update algorithm of strapdown inertial navigation, the recursive form of the inertial navigation specific force equation is:

[0099]

[0100] In the formula, , are the specific force velocities in the navigation system at , time respectively;

[0101] Step 2.4, the carrier position in the geodetic coordinate system at time can be obtained by the discrete form of the position update algorithm, which is as follows:

[0102]

[0103] wherein,

[0104] ,

[0105] wherein, denotes the position of the carrier in the terrestrial coordinate system at the time instant t; denotes an intermediate value of the position update matrix, is obtained by extrapolation, denotes the position-velocity, the index denotes and an intermediate time instant between the time instants t and t+1 ; is the calculation period; is the radius of curvature of the meridian circle in which the carrier is located, is the radius of curvature of the prime vertical circle in which the carrier is located; is the longitude in which the carrier is located at the time instant t, is the radius of curvature of the prime vertical circle in which the carrier is located at the time instant t, is the altitude above sea level in which the carrier is located at the time instant t; is the longitude in which the carrier is located at the time instant t+1, is the latitude in which the carrier is located at the time instant t+1, is the altitude above sea level in which the carrier is located at the time instant t+1.

[0106] Step 3, a discrete position candidate point array is constructed with the prior position obtained by the strapdown inertial navigation as the center, the light quantum pulse sequence that should be received is calculated according to the geometric position relationship between each candidate position and the known base station, and the correlation operation is performed with the actual received sequence, the candidate position with the maximum correlation value is output as the direct position estimation result, which is specifically as follows:

[0107] Step 3.1, the position obtained from the strapdown inertial navigation system is taken as the prior position, and a discrete position candidate point array is constructed in the three-dimensional space with the prior position as the center, as shown in FIG. 1, the interval between adjacent candidate points in the array is 5 cm, forming a cubic array with a side length of 1 m; Figure 3

[0108] ​​Step 3.2, at each candidate position, according to the geometric position relationship between it and the laser base station of known position, the time difference between the light quantum pulses from the four base stations is calculated, a 4 single pulse signal sequence with a time length of 1 ms corresponding to the same time is constructed according to the time difference, and the 4 sequences are spliced into a calculated signal sequence with a time length of 4 ms in the order of the base station serial number ; if the base station arrives at the carrier with a time difference of , then the interval between the th pulse and the th pulse in the calculated signal sequence is ms;

[0109] Step 3.3, the sequences obtained by the light detectors of 4 different wavelengths are cut and blocked at the same time according to the time interval of 1 ms, and it is ensured that the light pulse signals emitted by the base stations at the same time fall in the same time block, as shown in Figure 4 , the 4 sequence blocks in the time period of the positioning time are taken out, spliced according to the base station serial number identified by the wavelength, and the actual received sequence with a time length of 1 ms is formed

[0110] Step 3.4, the calculated signal sequence and the actual received sequence are correlated, and the correlation function can be defined as the following discrete form:

[0111]

[0112] In the formula, , the correlation function of the actual received sequence and the calculated signal sequence is represented; is the translation amount, which represents that the calculated signal sequence is translated rightward by sampling points; is the number of sampling points contained in 1 ms, is the sequence obtained by translating the calculated signal sequence rightward, i.e. lagging behind, by sampling time intervals;

[0113] Step 3.5, the maximum value of the correlation function is taken as the correlation value result of the candidate position;

[0114] Step 3.6, while traversing all candidate positions, if the correlation value of the candidate position is greater than the correlation value of the last candidate position, the stored correlation value and the position information are updated;

[0115] Step 3.7, the candidate position with the maximum correlation value is output, and after being converted into the position coordinates in the geodetic coordinate system, the position measurement value of the direct position estimation is obtained​​ .

[0116] Step 4, combine the direct position estimation with the positioning result of the strapdown inertial navigation system into the Kalman filter for combined filtering to obtain the optimal estimation of the position, and compensate the sensor error of the inertial measurement unit according to the optimal estimation, as shown in Figure 5 .

[0117] Step 4.1, estimate the error quantity in the system by the error-state Kalman filter (ESKF) and feedback the estimated value as the correction quantity into the system to realize combined correction; the rod error between the optical detector and the inertial measurement unit is accurately measured and compensated in advance, and thus is not listed in the filter state; the state space equation of the integrated navigation system is:

[0118]

[0119] wherein

[0120]

[0121] , ,

[0122] In the formula, is the state vector of the integrated navigation system, , , are the attitude misalignment angles in the east, north and sky directions respectively; , , are the latitude error, longitude error and height error respectively; , , are the velocity errors in the east, north and sky directions respectively; , , are the random constant drifts of the gyroscope in the x, y and z axes in the carrier coordinate system respectively; , , are the zero-position drifts of the accelerometer in the x, y and z axes in the carrier coordinate system respectively; is the system state transition matrix; , , are the strapdown inertial navigation system error matrix, inertial device error conversion matrix and inertial device error matrix respectively; is the system noise driving matrix, is the attitude matrix of the carrier relative to the navigation coordinate system, is a system noise variable, is a gyro angular rate measurement white noise, is an accelerometer specific force measurement white noise;

[0123] Step 4.2, the integrated navigation system subtracts the position measurement value of the direct position estimation from the position measurement value of the strapdown inertial navigation system, and inputs the difference value as an observation into the filter for integrated filtering, and the observation equation is:

[0124]

[0125] wherein

[0126]

[0127] in the formula, is a system observation vector, is a position measurement vector of the strapdown inertial navigation system, is a position measurement vector of the direct position estimation; is a system observation matrix, is a position measurement white noise matrix; is a matrix of the strapdown inertial navigation position differential equation, is a velocity vector of the strapdown inertial navigation system at the current time calculated by the strapdown inertial navigation system;

[0128] Step 4.3, the attitude misalignment angle, the position error and the velocity error in the filter state vector are fed back to the strapdown inertial navigation system, the position and velocity parameter information of the strapdown inertial navigation system are corrected, and the corrected information is output as the optimal estimation result of the integrated navigation system, and the gyro random constant drift and the accelerometer zero drift obtained by the filter are used to compensate the sensor errors of the inertial measurement unit.

[0129] In this embodiment, the prior position information is obtained by the strapdown inertial navigation system, and a discrete position candidate point array is constructed with the prior position information as the center. According to the geometric position relationship between each candidate position and the known light pulse emission base station, the sequence to be received is calculated, and the correlation operation is performed with the actual received sequence, and then the direct position estimation result is obtained, and the integrated filtering is performed with the strapdown inertial navigation system positioning result, the optimal estimation of the position is obtained, and the sensor errors of the inertial measurement unit are compensated, so that the high-precision positioning of the three-dimensional scene is realized. In combination with the advantages of the laser intensity correlation positioning and the inertial navigation positioning, the absolute positioning is realized by using the prior information obtained by the strapdown inertial navigation based on the direct position estimation method, and the strapdown inertial navigation result is corrected, the deep coupling of the laser intensity correlation absolute positioning and the relative positioning of the strapdown inertial navigation is realized, and the precision and reliability of the long-time positioning and navigation are enhanced.

[0130] The above merely is the preferred embodiment of the present application, it should be pointed out that, for ordinary skilled in the art, without departing from the principles of the present application, can also make a number of improvements and refinements, these improvements and refinements should also be considered as the protection scope of the present application.

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 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. 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 moment. 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, 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: 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. .

6. The three-dimensional scene localization method based on laser intensity correlation and inertial navigation depth coupling according to claim 5, 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).

7. The three-dimensional scene localization method based on laser intensity correlation and inertial navigation depth coupling according to claim 6, 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.

8. 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 7, 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.

9. 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 7.

10. 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 7.

Images