A method and system for combined positioning of inertial navigation and data link in non-line-of-sight environment

Abstract

The present application relates to the field of radio navigation technology, and more particularly to a kind of inertial navigation and data link combined positioning method and system in non-line-of-sight environment, method includes: obtaining robot posture, speed and position information by inertial measurement unit;Data link communication ranging unit obtains the data link measured vector ranging value and position information of multiple adjacent robots with the robot in real time;Real-time obtain the data link calculated vector ranging value and vector distance change value between the robot and multiple adjacent robots;Obtain the distance estimate between the robot and multiple adjacent robots;Eliminate the data link measured vector ranging value of non-visual environment value;If the number of remaining data link measured vector ranging value does not meet the requirements, then reconstruct the data link measured vector ranging value with distance estimate;High-precision robot position information is obtained by using inertial navigation and data link combined positioning method model.The present application improves the data link ranging accuracy and the inertial navigation and data link combined navigation accuracy.

Description

Technical Field

[0001] This invention relates to the field of radio navigation technology, and in particular to a method and system for positioning using a combination of inertial navigation and data link in a non-line-of-sight environment. Background Technology

[0002] With the rapid development of intelligent transportation, drone inspection, indoor robots, and other fields, higher requirements are being placed on the positioning accuracy, continuity, and robustness of mobile vehicles. GNSS, as the mainstream positioning technology, can achieve meter-level or even centimeter-level positioning in open line-of-sight (LOS) environments. However, in non-line-of-sight (NLOS) environments such as indoor spaces, underground parking garages, urban canyons, and densely packed buildings, satellite navigation signals are easily affected by obstruction and multipath reflection, resulting in positioning drift, jumps, or even complete failure, making it difficult to meet the requirements for continuous positioning.

[0003] Inertial navigation systems (INS), with their advantages of complete autonomy, independence from external signals, and high dynamic response, can achieve continuous position, velocity, and attitude calculations, making them a core supplementary technology for positioning in NLOS environments. However, INS, based on dead reckoning, suffers from cumulative positioning errors over time. When used alone, significant positioning deviations can occur within a short period, failing to meet the demands for long-term, high-precision positioning. Wireless data links, as a short-range wireless communication technology, enable real-time information exchange among multiple nodes. Through distance observation between nodes, they can provide external positioning constraints and are far less affected by environmental obstructions than GNSS signals, making them an ideal external data source for INS error correction in indoor environments. However, data link signals are susceptible to NLOS problems such as reflections, leading to significantly increased ranging errors and impacting the accuracy of combined positioning. Summary of the Invention

[0004] The technical problem to be solved by the present invention is to provide a method and system for positioning by combining inertial navigation and data link in non-line-of-sight environment, which improves the ranging accuracy of data link, thereby improving the navigation accuracy of inertial navigation and data link combination, and providing high-precision navigation information guarantee for robot swarm operation.

[0005] This invention is achieved through the following technical solution: A method for positioning using a combination of inertial navigation and data link in a non-line-of-sight environment includes the following steps: S1: The inertial measurement unit collects the robot's angular velocity and acceleration information in real time, and obtains the robot's attitude, velocity and position information through the strapdown inertial navigation algorithm; S2: The data link communication ranging unit obtains the real-time measured vector ranging values ​​and position information of this robot and multiple adjacent robots via the data link; S3: Based on the robot's attitude, velocity, and position information calculated by inertial navigation and the position information of the robot and multiple neighboring robots obtained by the data link, the vector ranging value of the data link between the robot and multiple neighboring robots is obtained in real time. S4: Calculate the change in vector distance between this robot and multiple adjacent robots based on the robot velocity information of the next cycle calculated by strapdown inertial navigation; S5: Based on the data link between this robot and multiple adjacent robots, calculate the vector ranging value and the vector distance change value to obtain the estimated distance between this robot and multiple adjacent robots; S6: Calculate the error of the data link vector ranging value based on the measured vector ranging value and the distance estimate of the data link between this robot and multiple adjacent robots, and determine whether the measured vector ranging value of the data link is a non-visual environment value. If the measured vector ranging value of the data link is determined to be a non-visual environment value, then it is removed. S7: Determine whether the number of retained data link measured vector distance values ​​meets the requirements. If it does, proceed directly to the next step. If it does not meet the requirements, reconstruct the data link measured vector distance values ​​using the distance estimate, and double the error of the corresponding data link vector distance values ​​before proceeding to the next step. S8: Using the retained or reconstructed data link measured vector ranging values ​​as observations, a combined inertial navigation and data link positioning method model is adopted to obtain high-precision robot position information.

[0006] Furthermore, in step S1, the angular velocity and acceleration information of the robot collected by the inertial measurement unit are first preprocessed by the core processing unit to remove abnormal information, and then the robot's attitude, velocity, and position information are obtained through the strapdown inertial navigation algorithm.

[0007] Further, in step S3, the data link vector ranging values ​​between this robot and multiple adjacent robots are calculated according to equation (1): (1); in: express Time Robot and the First Data link vector ranging values ​​between adjacent robots express The robot that performs inertial navigation calculations at all times Position in the axial direction express The first data link obtained at time Neighboring robots Position in the axial direction express The robot that performs inertial navigation calculations at all times Position in the axial direction express The first data link obtained at time Neighboring robots Position in the axial direction express The robot that performs inertial navigation calculations at all times Position in the axial direction express The first data link obtained at time Neighboring robots Position along the axis.

[0008] Furthermore, in step S4, the vector distance change between this robot and multiple adjacent robots is calculated according to equation (2): (2); in: This represents the change in vector distance between this robot and multiple adjacent robots. express The robot that performs inertial navigation calculations at all times Velocity and position in the axial direction express The robot that performs inertial navigation calculations at all times Velocity and position in the axial direction express The robot that performs inertial navigation calculations at all times Velocity and position in the axial direction Indicates a time interval.

[0009] Furthermore, in step S5, the estimated distance between this robot and multiple adjacent robots is obtained according to equation (3): (3); in: express Time Robot and the First Estimated distance between adjacent robots express Time Robot and the First The data link between adjacent robots calculates the vector ranging value. This indicates that this robot is related to the first... The change in vector distance between adjacent robots.

[0010] Furthermore, in step S6, the error of the data link vector ranging value is calculated according to equation (4): (4); in: express Time Robot and the First Error in data link vector ranging values ​​between adjacent robots express Time Robot and the First Estimated distance between adjacent robots express Time Robot and the First Measured vector distance values ​​of the data links of adjacent robots.

[0011] Furthermore, the method for determining whether the measured vector ranging value of the data link is a non-visual environment value in step S6 is as follows: S611: Compare the calculated data link vector ranging value error with the set maximum error threshold. If the data link vector ranging value error is greater than the maximum error threshold, then determine that the actual measured vector ranging value of the data link is a non-visual environment value and remove the actual measured vector ranging value of the data link. S612: Compare the calculated data link vector ranging error with 4 times the average data link vector ranging error. If the data link vector ranging error is greater than 4 times the average data link vector ranging error, then the actual measured vector ranging value of the data link is determined to be a non-visual environment value, and the actual measured vector ranging value of the data link is discarded. S613: If the error in the data link vector ranging value between this robot and the second adjacent robot is greater than twice the sum of the errors in the data link vector ranging values ​​between this robot and the first adjacent robot and the first adjacent robot and the second adjacent robot, then the distance between this robot and the second adjacent robot is calculated based on the relative positional relationship of the adjacent robots. Then, the difference between the data link vector ranging value between this robot and the second adjacent robot and the distance between this robot and the second adjacent robot is calculated, and the difference is compared with a set threshold. If the difference is greater than the set threshold, then the measured vector ranging value of the data link is determined to be a non-visual environment value, and the measured vector ranging value of the data link is discarded.

[0012] Furthermore, in step S7, reconstructing the measured vector ranging value of the data link using the distance prediction value means replacing the measured vector ranging value of the data link with the distance prediction value before performing the next step of inertial navigation and data link combined positioning calculation. The requirement is that the number of measured vector ranging values ​​of the data link is greater than or equal to 3, and the requirement is that the number of measured vector ranging values ​​of the data link is less than 3.

[0013] Furthermore, the method for obtaining high-precision robot position information in step S8 is as follows: S811: The basic state equation of the inertial navigation and data link combined positioning method model is determined as equation (5): (5); in: This represents the prior estimate of the state variables. Represents the transition matrix. express Error of state quantity at time step Represents the noise distribution matrix. Represents a noise sequence; S812: Calculate the pseudorange difference according to equation (6). If it is necessary to reconstruct the data link vector ranging value using the distance estimate, then use the distance estimate instead. : (6); in: Indicates pseudo-distance difference, express The distance between robots is calculated using the position given by the inertial navigation system at all times. express Time Robot and the First Measured vector distance values ​​of the data links of adjacent robots; S813: Iterate according to the basic state equation, and make a one-step prediction according to equation (7): (7); in: Denotes the posterior covariance matrix. Denotes the prior covariance matrix. Represents the process noise covariance matrix. Indicates matrix transpose; S814: Based on one-step prediction, the Kalman gain equation is obtained as equation (8): (8); in: Represents the Kalman gain matrix. For the observation matrix, To measure the noise covariance matrix; S815: Based on the Kalman gain equation, update according to equation (9) to obtain the current... The state error at any given time is based on the current state. Calculating the state variable error at any given time to obtain high-precision robot position information: (9); in: express The covariance matrix at time t, Represents the identity matrix. express Error of state quantity at time step express The pseudo-distance matrix at time t, This indicates the number of distance measurements involved in the calculation.

[0014] A non-line-of-sight (NLS) inertial navigation and data link combined positioning system is provided to execute a non-line-of-sight (NLS) inertial navigation and data link combined positioning method as described in any of the above-mentioned methods. The system includes an inertial measurement unit, a data link communication ranging unit, a core processing unit, and an inertial navigation and data link combined positioning method model. The inertial measurement unit is used to collect the robot's angular velocity and acceleration information in real time, and obtain the robot's attitude, velocity and position information through the strapdown inertial navigation algorithm; The data link communication ranging unit is used to obtain the real-time measured vector ranging values ​​and position information of the data link between this robot and multiple adjacent robots; The core processing unit is used to calculate in real time the data link calculated vector distance values ​​between the robot and multiple adjacent robots, the vector distance change values ​​between the robot and multiple adjacent robots, and the estimated distance values ​​between the robot and multiple adjacent robots. It also determines whether the measured vector distance values ​​of the data link are non-visual environment values. If the measured vector distance values ​​of the data link are determined to be non-visual environment values, then the measured vector distance values ​​of the data link are removed. It also determines whether the number of retained measured vector distance values ​​of the data link meets the requirements. If the number of retained measured vector distance values ​​of the data link meets the requirements, then it directly proceeds to the next step. If the number of retained measured vector distance values ​​of the data link does not meet the requirements, then the measured vector distance values ​​of the data link are reconstructed using the estimated distance values, and the error of the corresponding measured vector distance values ​​is doubled before proceeding to the next step. The inertial navigation and data link combined positioning method model is used to obtain high-precision robot position information by using the retained actual data link vector ranging value or the reconstructed data link vector ranging value as the observation.

[0015] Beneficial effects of the invention: This invention provides a method and system for combined inertial navigation and data link positioning in non-line-of-sight (NLOS) environments. Addressing the complex environment of indoor robot swarm operations where GNSS receivers are unavailable and data link ranging errors exhibit NLOS (Not at Line of Reference) errors, this invention proposes a combined inertial navigation and data link navigation method suitable for this scenario. This invention fully utilizes the high short-term accuracy of inertial navigation and the fact that data link ranging errors do not accumulate over time. It constructs an NLOS ranging value detection method to eliminate NLOS ranging values ​​and simultaneously construct new ranging values, thereby improving the accuracy of data link ranging and ultimately enhancing the accuracy of combined inertial navigation and data link navigation, providing high-precision navigation information for robot swarm operations. Attached Figure Description

[0016] Figure 1 This is a schematic diagram of the overall process of the present invention.

[0017] Figure 2 This is a schematic diagram illustrating the relationship between the robot of this invention and its neighboring robots. Detailed Implementation

[0018] A method for positioning using a combination of inertial navigation and data link in a non-line-of-sight environment, the overall flowchart of which is shown in Figure 1, includes the following steps: S1: The inertial measurement unit collects the robot's angular velocity and acceleration information in real time, and obtains the robot's attitude, velocity and position information through the strapdown inertial navigation algorithm; Furthermore, the angular velocity and acceleration information of the robot collected by the inertial measurement unit is first preprocessed by the core processing unit to remove abnormal information, and then the robot's attitude, velocity and position information are obtained through the strapdown inertial navigation algorithm.

[0019] S2: The data link communication ranging unit obtains the real-time measured vector ranging values ​​and position information of this robot and multiple adjacent robots via the data link; Specifically, when robots work collaboratively in a swarm, the measured vector distance between two robots via the data link can be expressed as follows: ; in: This represents the distance measured between the two robots based on the data link. This represents the actual distance between the two robots. This indicates the ranging error caused by systematic errors, etc. This indicates the ranging error between the two robots caused by non-line-of-sight wireless signal transmission.

[0020] S3: Based on the robot's attitude, velocity, and position information calculated by inertial navigation and the position information of the robot and multiple neighboring robots obtained by the data link, the vector ranging value of the data link between the robot and multiple neighboring robots is obtained in real time. Taking advantage of the small short-term error of inertial navigation, the vector ranging value of the data link can be predicted using multi-source navigation information.

[0021] Specifically, the vector ranging value between this robot and multiple adjacent robots can be calculated according to equation (1): (1); in: express Time Robot and the First The data link between adjacent robots calculates the vector ranging value. express The robot that performs inertial navigation calculations at all times Position in the axial direction express The first data link obtained at time Neighboring robots Position in the axial direction express The robot that performs inertial navigation calculations at all times Position in the axial direction express The first data link obtained at time Neighboring robots Position in the axial direction express The robot that performs inertial navigation calculations at all times Position in the axial direction express The first data link obtained at time Neighboring robots Position along the axis.

[0022] S4: Calculate the change in vector distance between this robot and multiple adjacent robots based on the robot velocity information of the next cycle calculated by strapdown inertial navigation; Specifically, the vector distance change between this robot and multiple adjacent robots can be calculated according to equation (2): (2); in: This represents the change in vector distance between this robot and multiple adjacent robots. express The robot that performs inertial navigation calculations at all times Velocity and position in the axial direction express The robot that performs inertial navigation calculations at all times Velocity and position in the axial direction express The robot that performs inertial navigation calculations at all times Velocity and position in the axial direction Indicates a time interval.

[0023] S5: Based on the data link between this robot and multiple adjacent robots, calculate the vector ranging value and the vector distance change value to obtain the estimated distance between this robot and multiple adjacent robots; Specifically, the estimated distance between this robot and multiple adjacent robots can be obtained according to equation (3): (3); in: express Time Robot and the First Estimated distance between adjacent robots express Time Robot and the First The data link between adjacent robots calculates the vector ranging value. This indicates that this robot is related to the first... The change in vector distance between adjacent robots.

[0024] S6: Calculate the error of the data link vector ranging value based on the measured vector ranging value and distance estimate of the data link between this robot and multiple adjacent robots, and determine whether the measured vector ranging value of the data link is a non-visual environment value. If it is determined that the measured vector ranging value of the data link is a non-visual environment value, then the measured vector ranging value of the data link is removed. Specifically, the error of the data link vector ranging value can be calculated according to equation (4): (4); in: express Time Robot and the First Error in data link vector ranging values ​​between adjacent robots express Time Robot and the First Estimated distance between adjacent robots express Time Robot and the First Measured vector distance values ​​of the data links of adjacent robots.

[0025] The method for determining whether a measured vector distance value from a data link is a non-visual environment value is as follows: S611: Compare the calculated data link vector ranging value error with the set maximum error threshold. If the data link vector ranging value error is greater than the maximum error threshold, then determine that the actual measured vector ranging value of the data link is a non-visual environment value and remove the actual measured vector ranging value of the data link. S612: Compare the calculated data link vector ranging error with 4 times the average data link vector ranging error. If the data link vector ranging error is greater than 4 times the average data link vector ranging error, then the actual measured vector ranging value of the data link is determined to be a non-visual environment value, and the actual measured vector ranging value of the data link is discarded. Specifically, the data link vector ranging error can be calculated based on equation (4), and the data link vector ranging error vector can be obtained as follows: ;in: This represents the error vector of the data link vector ranging value. The number of robots is represented by the following formula, which is used to calculate the mean error of the data link vector ranging value. : .

[0026] S613: If the error in the data link vector ranging value between this robot and the second adjacent robot is greater than twice the sum of the errors in the data link vector ranging values ​​between this robot and the first adjacent robot and the first adjacent robot and the second adjacent robot, then the distance between this robot and the second adjacent robot is calculated based on the relative positional relationship of the adjacent robots. Then, the difference between the data link vector ranging value between this robot and the second adjacent robot and the distance between this robot and the second adjacent robot is calculated, and the difference is compared with a set threshold. If the difference is greater than the set threshold, then the measured vector ranging value of the data link is determined to be a non-visual environment value, and the measured vector ranging value of the data link is discarded.

[0027] The diagram illustrating the relationship between this robot and its neighboring robots is shown below. Figure 2 As shown in the figure express The measured vector distance between this robot and its first neighboring robot at any given moment. express The measured vector distance value of the data link between this robot and the second adjacent robot at that moment. express The measured vector distance value of the data link between the first adjacent robot and the second adjacent robot at any given time. express The angle between the line connecting this robot to the first adjacent robot and the line connecting the first adjacent robot to the second adjacent robot.

[0028] Specifically, it can be based on Calculate the distance between this robot and the second adjacent robot. ,according to Calculate the difference between the data link vector distance between this robot and the second adjacent robot and the distance between this robot and the second adjacent robot. .

[0029] S7: Determine whether the number of retained data link measured vector distance values ​​meets the requirements. If the number of retained data link measured vector distance values ​​meets the requirements, proceed directly to the next step. If the number of retained data link measured vector distance values ​​does not meet the requirements, reconstruct the data link measured vector distance values ​​using the distance estimate, and double the error of the corresponding data link vector distance values ​​before proceeding to the next step. Reconstructing the measured vector ranging value of the data link using the distance prediction value refers to replacing the measured vector ranging value of the data link with the distance prediction value before performing the next step of inertial navigation and data link combined positioning calculation.

[0030] The optimized approach allows setting the number of measured vector distance values ​​in the data link to be greater than or equal to 3 to meet the requirements, and less than 3 to not meet the requirements.

[0031] The requirement is that the number of data link vector ranging values ​​is greater than or equal to 3. If the number of retained data link vector ranging values ​​does not meet the requirement, the data link vector ranging values ​​are reconstructed using the distance estimate. The purpose is to ensure that the integrated navigation can continue to calculate.

[0032] Doubling the corresponding data link vector ranging error can reduce the contribution of the data link vector ranging error.

[0033] S8: Using the retained or reconstructed data link measured vector ranging values ​​as observations, a combined inertial navigation and data link positioning method model is adopted to obtain high-precision robot position information.

[0034] Specifically, the method for obtaining high-precision robot position information is as follows: S811: The basic state equation of the inertial navigation and data link combined positioning method model is determined as equation (5): (5); in: This represents the prior estimate of the state variables. Represents the transition matrix. express Error of state quantity at time step Represents the noise distribution matrix. Represents a noise sequence; , The inertial navigation system (INS) state error includes three-dimensional attitude error, three-dimensional velocity error, three-dimensional position error, three-dimensional gyroscope drift error, and three-dimensional gyroscope bias error. The INS state error can be expressed as follows: , Indicates the heading angle error. Indicates the pitch angle error. Indicates the roll angle error. This indicates the eastward velocity error of the inertial navigation system. This indicates the northbound velocity error of the inertial navigation system. This indicates the inertial navigation system's upward velocity error. This indicates the positional error in the latitude direction of the inertial navigation system. This indicates the positional error in the longitude direction of the inertial navigation system. This indicates the position error in the altitude direction of the inertial navigation system. Inertial navigation Axial gyroscope drift error, Inertial navigation Axial gyroscope drift error, This represents the gyroscope drift error in the z-axis direction of the inertial navigation system. Inertial navigation Add zero offset error to the axial direction. Inertial navigation Add zero offset error to the axial direction. This indicates the zero-bias error added to the z-axis direction of the inertial navigation system; Indicates data link error. , This represents the distance error equivalent to the data link clock error.

[0035] , Denotes the transfer matrix of the inertial navigation error equations, This represents the transition matrix of the data link error equation.

[0036] , Represents the inertial navigation noise allocation matrix. This represents the data link noise allocation matrix.

[0037] , Represents the inertial navigation noise sequence. This represents a data link noise sequence.

[0038] S812: Calculate the pseudorange difference according to equation (6). If it is necessary to reconstruct the measured vector distance value of the data link using the distance estimate, then use the distance estimate instead. : (6); in: Indicates pseudo-distance difference, express The distance between robots is calculated using the position given by the inertial navigation system at all times. express Time Robot and the First Measured vector distance values ​​of the data links of adjacent robots; S813: Iterate according to the basic state equation, and make a one-step prediction according to equation (7): (7); in: Denotes the posterior covariance matrix. Denotes the prior covariance matrix. Represents the process noise covariance matrix. Indicates matrix transpose; S814: Based on one-step prediction, the Kalman gain equation is obtained as equation (8): (8); in: Represents the Kalman gain matrix. For the observation matrix, To measure the noise covariance matrix; S815: Based on the Kalman gain equation, update according to equation (9) to obtain the current... The state error at any given time is based on the current state. Calculating the state variable error at any given time to obtain high-precision robot position information: (9); in: express The covariance matrix at time t, Represents the identity matrix. express Error of state quantity at time step express The pseudo-distance matrix at time t, This indicates the number of distance measurements involved in the calculation.

[0039] The method provided by this invention addresses the complex environment of indoor robot swarm operations, where GNSS receivers are unavailable and data link ranging exhibits NLOS (Not In-Loop Omission) errors. It fully leverages the high short-term accuracy of inertial navigation and the fact that data link ranging errors do not accumulate over time to construct an NLOS ranging value detection method. This method eliminates NLOS ranging values ​​while simultaneously constructing new ranging values, improving the accuracy of data link ranging and thus enhancing the accuracy of combined inertial navigation and data link navigation. This provides high-precision navigation information assurance for robot swarm operations.

[0040] A non-line-of-sight (NLS) inertial navigation and data link combined positioning system is provided to execute a non-line-of-sight (NLS) inertial navigation and data link combined positioning method as described in any of the above-mentioned methods. The system includes an inertial measurement unit, a data link communication ranging unit, a core processing unit, and an inertial navigation and data link combined positioning method model. The inertial measurement unit is used to collect the robot's angular velocity and acceleration information in real time, and obtain the robot's attitude, velocity and position information through the strapdown inertial navigation algorithm; The data link communication ranging unit is used to obtain the real-time measured vector ranging values ​​and position information of the data link between this robot and multiple adjacent robots; The core processing unit is used to calculate in real time the data link calculated vector distance values ​​between the robot and multiple adjacent robots, the vector distance change values ​​between the robot and multiple adjacent robots, and the estimated distance values ​​between the robot and multiple adjacent robots. It also determines whether the measured vector distance values ​​of the data link are non-visual environment values. If the measured vector distance values ​​of the data link are determined to be non-visual environment values, then the measured vector distance values ​​of the data link are removed. It also determines whether the number of retained measured vector distance values ​​of the data link meets the requirements. If the number of retained measured vector distance values ​​of the data link meets the requirements, then it directly proceeds to the next step. If the number of retained measured vector distance values ​​of the data link does not meet the requirements, then the measured vector distance values ​​of the data link are reconstructed using the estimated distance values, and the error of the corresponding measured vector distance values ​​is doubled before proceeding to the next step. The inertial navigation and data link combined positioning method model is used to obtain high-precision robot position information by using the retained actual data link vector ranging value or the reconstructed data link vector ranging value as the observation.

[0041] In summary, the present invention provides a method and system for positioning using inertial navigation and data link in a non-line-of-sight environment, which improves the ranging accuracy of data link and the navigation accuracy of inertial navigation and data link combined, and provides high-precision navigation information for robot swarm operations.

[0042] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A method for positioning using a combination of inertial navigation and data link in a non-line-of-sight environment, characterized in that: Includes the following steps: S1: The inertial measurement unit collects the robot's angular velocity and acceleration information in real time, and obtains the robot's attitude, velocity and position information through the strapdown inertial navigation algorithm; S2: The data link communication ranging unit obtains the real-time measured vector ranging values ​​and position information of this robot and multiple adjacent robots via the data link; S3: Based on the robot's attitude, velocity, and position information calculated by inertial navigation and the position information of the robot and multiple neighboring robots obtained by the data link, the vector ranging value of the data link between the robot and multiple neighboring robots is obtained in real time. S4: Calculate the change in vector distance between this robot and multiple adjacent robots based on the robot velocity information of the next cycle calculated by strapdown inertial navigation; S5: Based on the data link between this robot and multiple adjacent robots, calculate the vector ranging value and the vector distance change value to obtain the estimated distance between this robot and multiple adjacent robots; S6: Calculate the error of the data link vector ranging value based on the measured vector ranging value and the distance estimate of the data link between this robot and multiple adjacent robots, and determine whether the measured vector ranging value of the data link is a non-visual environment value. If the measured vector ranging value of the data link is determined to be a non-visual environment value, then it is removed. S7: Determine whether the number of retained data link measured vector distance values ​​meets the requirements. If it does, proceed directly to the next step. If it does not meet the requirements, reconstruct the data link measured vector distance values ​​using the distance estimate, and double the error of the corresponding data link vector distance values ​​before proceeding to the next step. S8: Using the retained or reconstructed data link measured vector ranging values ​​as observations, a combined inertial navigation and data link positioning method model is adopted to obtain high-precision robot position information.

2. The inertial navigation and data link combined positioning method in a non-line-of-sight environment according to claim 1, characterized in that: In step S1, the angular velocity and acceleration information of the robot collected by the inertial measurement unit are first preprocessed by the core processing unit to remove abnormal information, and then the robot's attitude, velocity and position information are obtained through the strapdown inertial navigation algorithm.

3. The inertial navigation and data link combined positioning method in a non-line-of-sight environment according to claim 1, characterized in that: Step S3 calculates the vector ranging value of the data link between this robot and multiple adjacent robots according to equation (1): （1）； in: express Time Robot and the First The data link between adjacent robots calculates the vector ranging value. express The robot that performs inertial navigation calculations at all times Position in the axial direction express The first data link obtained at time Neighboring robots Position in the axial direction express The robot that performs inertial navigation calculations at all times Position in the axial direction express The first data link obtained at time Neighboring robots Position in the axial direction express The robot that performs inertial navigation calculations at all times Position in the axial direction express The first data link obtained at a certain time Neighboring robots Position along the axis.

4. The inertial navigation and data link combined positioning method in a non-line-of-sight environment according to claim 1, characterized in that: In step S4, the vector distance change between this robot and multiple adjacent robots is calculated according to equation (2): （2）； in: This represents the change in vector distance between this robot and multiple adjacent robots. express The robot that performs inertial navigation calculations at all times Velocity and position in the axial direction express The robot that performs inertial navigation calculations at all times Velocity and position in the axial direction express The robot that performs inertial navigation calculations at all times Velocity and position in the axial direction Indicates a time interval.

5. The inertial navigation and data link combined positioning method in a non-line-of-sight environment according to claim 1, characterized in that: In step S5, the estimated distance between this robot and multiple adjacent robots is obtained according to equation (3): （3）； in: express Time Robot and the First Estimated distance between adjacent robots express Time Robot and the First The data link between adjacent robots calculates the vector ranging value. This indicates that this robot is related to the first... The change in vector distance between adjacent robots.

6. The inertial navigation and data link combined positioning method in a non-line-of-sight environment according to claim 1, characterized in that: In step S6, the error of the data link vector ranging value is calculated according to equation (4): （4）； in: express Time Robot and the First Error in data link vector ranging values ​​between adjacent robots express Time Robot and the First Estimated distance between adjacent robots express Time Robot and the First Measured vector distance values ​​of the data links of adjacent robots.

7. The inertial navigation and data link combined positioning method in a non-line-of-sight environment according to claim 1, characterized in that: The method for determining whether the measured vector ranging value of the data link is a non-visual environment value in step S6 is as follows: S611: Compare the calculated data link vector ranging value error with the set maximum error threshold. If the data link vector ranging value error is greater than the maximum error threshold, then determine that the actual measured vector ranging value of the data link is a non-visual environment value and remove the actual measured vector ranging value of the data link. S612: Compare the calculated data link vector ranging error with 4 times the average data link vector ranging error. If the data link vector ranging error is greater than 4 times the average data link vector ranging error, then the actual measured vector ranging value of the data link is determined to be a non-visual environment value, and the actual measured vector ranging value of the data link is discarded. S613: If the data link vector ranging error between this robot and the second adjacent robot is greater than twice the sum of the data link vector ranging errors between this robot and the first adjacent robot and the data link vector ranging errors between the first and second adjacent robots, then the distance between this robot and the second adjacent robot is calculated based on the relative positional relationship of the adjacent robots. Then, the difference between the actual measured vector ranging value of the data link between this robot and the second adjacent robot and the distance between this robot and the second adjacent robot is calculated, and the difference is compared with a set threshold. If the difference is greater than the set threshold, then the actual measured vector ranging value of the data link is determined to be a non-visual environment value, and the actual measured vector ranging value of the data link is removed.

8. The inertial navigation and data link combined positioning method in a non-line-of-sight environment according to claim 1, characterized in that: In step S7, reconstructing the data link vector ranging value using the distance prediction value means replacing the actual data link vector ranging value with the distance prediction value before performing the next step of inertial navigation and data link combined positioning calculation. The requirement is that the number of actual data link vector ranging values ​​is greater than or equal to 3, and the requirement is that the number of actual data link vector ranging values ​​is less than 3.

9. The inertial navigation and data link combined positioning method in a non-line-of-sight environment according to claim 1, characterized in that: The method for obtaining high-precision robot position information in step S8 is as follows: S811: The basic state equation of the inertial navigation and data link combined positioning method model is determined as equation (5): （5）； in: This represents the prior estimate of the state variables. Represents the transition matrix. express Error of state quantity at time step Represents the noise distribution matrix. Represents a noise sequence; S812: Calculate the pseudorange difference according to equation (6). If it is necessary to reconstruct the data chain using the distance estimate to calculate the vector ranging value, then use the distance estimate instead. : （6）； in: Indicates pseudo-distance difference, express The distance between robots is calculated using the position given by the inertial navigation system at all times. express Time Robot and the First Measured vector ranging values ​​of the data links of adjacent robots; S813: Iterate according to the basic state equation, and make a one-step prediction according to equation (7): （7）； in: Denotes the posterior covariance matrix. Denotes the prior covariance matrix. Represents the process noise covariance matrix. Indicates matrix transpose; S814: Based on one-step prediction, the Kalman gain equation is obtained as equation (8): （8）； in: Represents the Kalman gain matrix. For the observation matrix, To measure the noise covariance matrix; S815: Based on the Kalman gain equation, update according to equation (9) to obtain the current... The error of the state quantity at any given time is based on the current state. Calculating the state variable error at any given time to obtain high-precision robot position information: （9）； in: express The covariance matrix at time t, Represents the identity matrix. express Error of state quantity at time step express The pseudo-distance matrix at time t, This indicates the number of distance measurements involved in the calculation.

10. A positioning system combining inertial navigation and data link in a non-line-of-sight environment, characterized in that: The method for performing a combined inertial navigation and data link positioning method in a non-line-of-sight environment as described in any one of claims 1 to 9 includes an inertial measurement unit, a data link communication ranging unit, a core processing unit, and a combined inertial navigation and data link positioning method model. The inertial measurement unit is used to collect the robot's angular velocity and acceleration information in real time, and obtain the robot's attitude, velocity and position information through the strapdown inertial navigation algorithm; The data link communication ranging unit is used to obtain the real-time measured vector ranging values ​​and position information of the data link between this robot and multiple adjacent robots; The core processing unit is used to calculate in real time the data link calculated vector distance values ​​between the robot and multiple adjacent robots, the vector distance change values ​​between the robot and multiple adjacent robots, and the estimated distance values ​​between the robot and multiple adjacent robots. It also determines whether the measured vector distance values ​​of the data link are non-visual environment values. If the measured vector distance values ​​of the data link are determined to be non-visual environment values, then the measured vector distance values ​​of the data link are removed. It also determines whether the number of retained measured vector distance values ​​of the data link meets the requirements. If the number of retained measured vector distance values ​​of the data link meets the requirements, then it directly proceeds to the next step. If the number of retained measured vector distance values ​​of the data link does not meet the requirements, then the measured vector distance values ​​of the data link are reconstructed using the estimated distance values, and the error of the corresponding measured vector distance values ​​is doubled before proceeding to the next step. The inertial navigation and data link combined positioning method model is used to obtain high-precision robot position information by using the retained actual data link vector ranging value or the reconstructed data link vector ranging value as the observation.

Images