Autonomous positioning method of unmanned system based on integration of microwave distance measurement and inertial navigation

Abstract

The invention discloses an autonomous positioning method of an unmanned system based on integration of microwave distance measurement and inertial navigation, and belongs to the field of application of a cross technology of navigation positioning and signal processing. In an unmanned system, a filter is tightly combined with an inertial navigation system and a microwave distance measurement positioning system and is regarded as a navigation positioning system, a motion track of a target carrier is calculated, and a sensor observation model fusing two independent inertial navigation systems of inertial navigation and micro navigation in the navigation positioning system is constructed; the method comprises the following steps of: reflecting an interference result of a microwave signal and a reference signal by a solid-state spin detection object, and performing FFT (Fast Fourier Transform) on each reflected microwave signal after frequency mixing to finish microwave distance measurement; on the basis of forming distance calculation, position results output by a distance measurement positioning algorithm are fused, random errors of the system are compensated by adopting an error compensation algorithm, errors of the inertial navigation system are corrected, covariance is updated and corrected according to an observation matrix and Kalman gain, and intelligent autonomous control positioning of the system is realized.

Description

Technical Field

[0001] The present invention belongs to the cross-technology application field of navigation positioning and signal processing, and relates to a positioning technology that fuses microwave ranging reception signals with inertial navigation in a complex environment. In particular, it relates to an autonomous positioning method based on the fusion of microwave ranging and inertial navigation for an unmanned system that can fuse satellite positioning and inertial navigation for positioning. Background Art

[0002] Navigation and location services are cutting-edge technologies that integrate multiple disciplines such as navigation and positioning, 5G / 6G, artificial intelligence, the Internet of Things, computer science, and earth science. Combined positioning technology is a branch of navigation and positioning technology. In recent years, with the rapid development of society and the rapid growth of data services, the demand for accurate positioning and navigation in various fields of modern society has increased, which has not only led to the vigorous development of location-based services (LBS), but also prompted the accuracy, reliability and continuity of navigation and positioning technology. However, with the development of cities and the improvement of construction levels, there are more and more large and complex buildings. For these complex environments, the signal penetration ability of satellite navigation systems is weak, and its signal energy and received signal-to-noise ratio are greatly deteriorated compared to ordinary environments. The availability and overall performance of the signal are greatly reduced. In many environments, the positioning accuracy of the receiver is very poor, and in some cases it is even impossible to locate. Since satellite signals cannot penetrate most obstacles, satellite signals may not be able to be located at all due to noise and shielding. Therefore, it is difficult to meet the needs by relying solely on satellite positioning technology. Therefore, other positioning methods and means are particularly important as a substitute and supplement to satellite positioning technology in this era of the Internet of Everything. This leads to the two topics of inertial navigation technology and multi-source information fusion technology. Although the fusion of the global satellite positioning system (GPS) and the inertial navigation system (INS) has been used as a classic application of theory, and this fusion technology can work well in most cases and achieve excellent working results, the above fusion technology is obviously not usable in places or scenes where the GPS signal is lost. Although the combined positioning of wireless ranging and inertial navigation system (IS) can effectively improve the positioning accuracy of the system in the navigation of representative unmanned systems such as vehicles, drones, unmanned vehicles, unmanned ships, and robots. However, when the global satellite positioning system (GPS) loses lock during positioning, the positioning error caused by the long-term independent work of S, theoretically proves that the INS measurement coordinates obtained by wireless ranging only contain random drift errors, but the inertial navigation error will continue to accumulate over time, and the cumulative error effect limits its scope of use. In practical applications, the positioning error generated by S cannot be ignored. A typical Markov process is explained by statistical significance:

[0003] 1) The mathematical expectation of the system state is the true position of the system, but since the required positioning information is a realization of a certain random process, the error cannot be simply replaced by the mathematical expectation of the error;

[0004] 2) If the change of time t is taken into account, although the error distribution at each moment can be expressed more accurately in theory, the error distribution expression will be very complicated and not suitable for practical application. There is also the problem of error accumulation over time.

[0005] In a complex wireless communication environment, non-line-of-sight (NLOS) propagation caused by physical entity obstruction is an important factor affecting positioning accuracy. In an NLOS environment, propagation signals are easily obstructed by physical entities, resulting in positioning errors. Due to the complexity of the wireless communication environment, it is difficult to obtain good positioning performance by relying solely on positioning algorithms based on respective measurement quantities. There are generally shortcomings such as poor positioning effect, sudden jumps in positioning data, and a narrow range of applicable scenarios, which cannot effectively improve the positioning effect in an NLOS environment. How to reduce non-line-of-sight NLOS? Because the positioning environment is very complex, different buildings, different indoor layouts, patterns, materials, and building sizes will have different requirements for positioning technology.

[0006] In positioning technology, the positioning algorithms used can be divided into non-range NLOS-based and range-based positioning algorithms. Non-range-based algorithms include centroid algorithm, DV-Hop algorithm, approximate triangle interior point test (APIT) algorithm, etc. Relatively speaking, the more commonly used positioning algorithms are based on ranging. Depending on the measurement method, ranging-based positioning algorithms can include positioning based on time of arrival (TOA), time difference of arrival (TDOA), angle of arrival (AOA), and received signal strength identification (RSSI). Signals in wireless positioning systems based on TOA are easily affected by non-line-of-sight propagation. In pure inertial navigation, zero bias error has a greater impact on the positioning and navigation accuracy of medium and short distances, and calibration cannot be achieved. It is difficult to eliminate this error by relying solely on inertial navigation.

[0007] The process of using the inertial sensor data provided by IMU to extract acceleration and angular velocity based on the dead reckoning (PDR) algorithm and integrate the wireless positioning system to calculate the trajectory and posture of the vehicle is called the front end, and the back end is to optimize all the data. Back-end optimization technologies such as posture graphs can only be performed based on the constraints between the data sampled each time by the front end, so there is still a cumulative error. Moreover, if we only calculate the motion trajectory of the vehicle based on MU, the adverse effects caused by the cumulative error will be more serious. In IMU, since the sampling rate is generally above 100Hz, the back-end optimization calculation is extremely large, and it is generally impossible to implement it online in embedded systems, so there are few cases of using back-end optimization technology in inertial navigation. In the middle of the inertial navigation system, the main cumulative error comes from the random zero bias generated at the moment of MU startup. The zero bias cannot be calibrated and is different every time it is turned on, which has always been a problem that plagues inertial navigation. However, due to the limitations of sensors, it is not easy for inertial navigation to determine whether the positions between two locations are the same. Therefore, when using inertial navigation positioning alone, there will be a problem of not being able to know its initial position. At the same time, since the algorithm adopts the cumulative form, there is also a problem of gradually increasing cumulative errors.

[0008] MEMS inertial navigation technology based on micro-electromechanical system (MEMS) inertial sensors is an important branch of inertial navigation technology. At present, the navigation of drones mainly adopts the means of combining MEMS inertial navigation system with GPS. Due to the low accuracy of MEMS inertial devices, long-term use will cause the error to diverge quickly and cannot be used for long-term navigation tasks. The micro-inertial navigation system based on MEMS has navigation errors accumulated over time. The navigation accuracy of the imaging system needs to be corrected in time to continuously navigate and locate; the trajectory of inertial navigation is based on the trajectory calculated from the starting point and the starting direction. The deviation of the starting point position may cause the deviation of the overall trajectory, which brings certain difficulties to positioning. Therefore, multi-sensor fusion is generally used for navigation, that is, MEMS inertial navigation is integrated with other navigation methods, and the error of the inertial navigation system is corrected by the navigation information of other navigation systems, thereby improving the accuracy of the entire navigation system. If data fusion of multiple navigation systems is to be performed, filtering and other methods should be used. In addition, positioning technology based on micro-inertial navigation faces a series of difficulties. Micro-INS autonomous positioning technology uses MEMS technology's inertial measurement unit (including accelerometers and gyroscopes) to measure pedestrian motion acceleration and angular velocity, and solves speed, position and heading through integration. This technology is usually used in foot-mounted micro-INS positioning systems. Due to the imperfect manufacturing technology of MEMS inertial sensors, the measurement accuracy of MEMS inertial sensors is relatively poor. Micro-INS-based positioning technology is a completely autonomous positioning method that does not require any equipment to be installed in the positioning location in advance. It has the advantages of low cost, good reliability and no dependence. However, this technology also has disadvantages. The shoe-mounted micro-INS positioning technology uses integration to calculate speed and position, which is prone to error accumulation. In order to avoid continuous accumulation of errors, closed-loop detection combined with back-end optimization is often used to correct the accumulated errors and associate the trajectories. If the entire trajectory is not associated, the entire vehicle is equivalent to running in an endless corridor, which is meaningless for our navigation. That is, in a local area, incremental trajectories are continuously obtained, but over a long period of time, the vehicle cannot distinguish where the real direction is and how it is related to the origin at this moment.

[0009] The trajectory of inertial navigation is a trajectory calculated based on the starting point and the starting direction. The deviation of the starting point may cause the deviation of the overall trajectory, which brings certain difficulties to positioning. A common method of INS / GNSS fusion is to expand the Kalman filter. If the GNSS signal is not restored in time for a long time, the error accumulation caused by the zero bias of the inertial navigation system will inevitably accumulate slowly over time. For information fusion, the Kalman filter theory has been widely used. The fusion of the global satellite positioning system (GPS) and the inertial navigation system (INS) is a classic application of this theory. This fusion technology can work well in most cases and obtain excellent working results. However, in places or scenes where the GPS signal is lost, the above fusion technology is obviously not available. If the INS works independently for a long time, the cumulative effect of the error generated over time will be very obvious, resulting in the inability to accurately find the specific location of the vehicle during this period of time. In response to this problem, the Journal of Beijing University of Aeronautics and Astronautics, 2006, 32 (11) published a new approach to fleet network positioning. The document introduces distance into INS and derives the corresponding Kalman filter, which is suitable for situations where there are several vehicles in space and the relative position relationship between the vehicles is relatively unchanged, just as the fleet network positioning described in the document. However, in the case of vehicle navigation, the above solution is limited in application, and when INS cannot accurately locate, the Kalman filter cannot be used. Although the traditional external positioning method based on the global positioning system is highly accurate, the conditions for use are limited.

[0010] Radar positioning technology based on microwave signal measurement has been widely used in activities such as autonomous driving, intelligent production, health testing, and geological exploration. Microwaves are electromagnetic waves with a wavelength between 0.1m and 1mm. Microwave ranging has the advantages of high measurement accuracy, fast response speed, good directionality, and non-contact, which makes its application field very wide. The principle of microwave ranging is to calculate the distance by calculating the time difference taken for the electromagnetic wave to start from the instrument and return to the instrument after being reflected by the target. The principle formula is D = cΔt / 2 (D: distance, c: speed of light, Δt: time difference). Due to differences in manufacturing costs, its accuracy and range are different depending on the direction of use. The accuracy of professional rangefinders can reach mm level. The main function of the microwave ranging solution is to realize early warning and detection and ranging of low-altitude and ultra-low-altitude targets. The micron-level microwave ranging system is the core payload of the low-low satellite-satellite tracking gravity detection satellite and the main data source for inverting the earth's gravity field. The measurement accuracy of the inter-satellite distance change is at the micron level, and the delay stability requirements of the system's active components are very high. When using microwave rangefinders to measure distances, the influence of ground reflection is a serious obstacle to improving the accuracy of distance measurement, which is an urgent problem to be solved. In the process of microwave rangefinder distance measurement, there are many factors that affect the accuracy of distance measurement, including atmospheric medium (humidity, temperature, atmospheric pressure, etc.), zero point error, offset value, phase measurement error, counting clock frequency drift error, etc. These factors have different effects on the accuracy of distance measurement. From the perspective of distance measurement: the loss of microwaves in the air is much smaller than that of ultrasonic waves, and they usually have advantages above 15 meters, while ultrasonic waves have more advantages below 10 meters (a typical example is reversing radar). For obstacles, microwaves and ultrasonic waves can both detect multiple targets, but if the target is completely blocked, other distance measurement technologies such as ultrasonic waves will all be useless, but microwaves have a diffraction effect, and microwaves can "turn corners"! That is, it is possible to detect a part of the target that is completely blocked. But this effect sometimes brings many problems, depending on how it is used. In short, microwave ranging has many limitations. Microwaves as carrier waves Electromagnetic wave ranging The method is to place the main and auxiliary instruments at both ends of the survey line. The ranging signal transmitted by the main station is received by the auxiliary station and then forwarded to the main station by the auxiliary station. When the main station transmits a signal, the auxiliary station also transmits a signal to the main station at the same time. After mixing processing, the delay caused by the signal transmitted by the main station to go back and forth on the survey line can be calculated. Phase Difference , and then deduce the distance to be measured.

[0011] Inertial navigation positioning technology is a positioning technology based on the principle of inertial navigation. It uses inertial sensors (such as accelerometers and gyroscopes) to measure the acceleration and angular velocity of moving objects, and calculates the speed and position information of the object through integration and calculation methods, thereby achieving the purpose of positioning. It is a relatively independent positioning technology that does not need to rely on external signal sources (such as satellite signals or ground base station signals), so it has good anti-interference and reliability. The main application areas of this technology include aviation, aerospace, navigation, and other fields, as well as high-end smart cars, robots and other fields. Traditional satellite positioning technology cannot fully meet the needs in these fields with high positioning requirements, whether in terms of accuracy or coverage. For example, the positioning accuracy required for unmanned driving is often at the centimeter level. The currently popular fusion inertial navigation positioning technology, inertial navigation positioning technology, is a positioning technology based on the principle of inertial navigation. It uses inertial sensors (such as accelerometers and gyroscopes) to measure the acceleration and angular velocity of moving objects, and infers the speed and position information of the object through integration and calculation. Due to the existence of sensor deviation and noise, the trajectory of inertial navigation begins to deviate from the actual walking trajectory at about 100 degrees in the y-axis direction, and the degree of deviation increases with the distance traveled. The main reason for the deviation of inertial navigation from the actual trajectory is the double integration of the noise and deviation of the sensor (accelerometer and gyroscope) over time, among which the deviation has a more obvious impact on inertial navigation. Summary of the invention

[0012] In order to cope with the situation of unstable signals during fusion and improve the stability of the fusion system, the present invention aims at the problems and shortcomings of the prior art and provides an autonomous positioning method based on inertial navigation fusion microwave ranging, which has good fusion effect, low calculation complexity, fast speed, high positioning accuracy and timely transmission.

[0013] The solution adopted by the present invention to solve the existing technical problems is: an autonomous positioning method for an unmanned system based on the fusion of microwave ranging and inertial navigation, which has the following technical features:

[0014] In unmanned systems, the Kalman filter is tightly combined with the inertial navigation system (INS) and the microwave ranging positioning system, and regarded as a navigation and positioning system. The results based on microwave ranging positioning are integrated with the inertial navigation. In the form of pseudo code, the ranging positioning algorithm that integrates microwave radar and inertial navigation system is used to infer the motion trajectory of the target carrier. The result of integrating the inertial navigation positioning trajectory is always near the real trajectory. The motion process is modeled, and a sensor observation model that integrates two independent inertial navigation systems of inertial micro navigation in the navigation and positioning system is constructed. The output of the observation value of the sensor observation model is used as the input of the Kalman filter, and the state quantity that needs to be estimated is used as the output.

[0015] The composite microwave antenna composed of microwave spin quantum sensors measures information, collects and converges microwave signals propagating in free space into nanospace, measures the range of microwave signals by detecting the state of local solid-state quantum probes, and uses the Doppler effect principle to detect the speed and depth information of the target, converts the detection of weak signals in free space into the detection of the interaction between electromagnetic fields and solid-state spins at the nanoscale, detects the interference between the microwave signal reflected by the object and the reference signal through solid-state spins, mixes each reflected microwave signal and performs FFT to obtain the phase of the microwave signal reflected by the object, as well as the position information and transmission distance of the object, and completes microwave ranging. At the same time, multiple coherent interactions between solid-state spin quantum probes and microwave photons are used to achieve quantum-enhanced position measurement accuracy.

[0016] The acceleration and angular velocity data collected by the micro-inertial navigation system are extracted to calculate the trajectory and position of the target vehicle. Based on the system noise and observation noise, the system state at the next moment is estimated through the observation value at the previous moment, and the current state is updated progressively.

[0017] In the process of digital signal processing, the time signal is converted into a spectrum through fast FFT transformation, which forms the basis of distance calculation. Then, the external acceleration is inferred by calculating the object position distance / height, and the position results output by the ranging positioning algorithm are integrated to gradually pull the results of inertial navigation to the vicinity of ranging positioning, correct the cumulative errors in displacement and direction of the continuous trajectory generated by inertial navigation, use the error compensation algorithm to compensate for the random error of the system, correct the error of the inertial navigation system, and update and correct the covariance according to the observation matrix and Kalman gain to realize the intelligent autonomous control positioning of the system.

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

[0019] In the unmanned system, the present invention tightly combines the Kalman filter with the inertial navigation system (INS) and the microwave ranging positioning system, and regards them as the navigation and positioning system, thereby suppressing the error accumulation of the strapdown inertial navigation system and the volatility of the wireless positioning system, and achieving a better positioning effect. Under the condition that the inertial navigation system (INS) and the microwave ranging positioning system can work normally, the position error can be controlled within 0.5m. The result based on microwave ranging positioning is integrated with the real navigation, and the motion trajectory of the target carrier is calculated by the ranging positioning algorithm integrated with the microwave radar and the inertial navigation system in the form of pseudo code, which integrates the inertial navigation and the ranging positioning information. The ranging positioning algorithm has low computational complexity, fast speed and good fusion effect. The simulation results show that the positioning trajectory of the ranging positioning algorithm is smooth, with small error and no cumulative error. The result of integrating the inertial navigation positioning trajectory is always near the real trajectory, and a good fusion effect is obtained.

[0020] The present invention utilizes the respective advantages of its inertial navigation system (INS) and microwave ranging positioning system. The fusion result has a high spatial resolution and a high overlap of the fusion motion trajectory path. The sensor observation model of the navigation and positioning system constructed by integrating two independent inertial navigation systems, the inertial micro navigation, can more intuitively observe the trajectory distortion caused by the accumulation of zero bias errors. A comparative experiment was carried out based on the classic inertial navigation algorithm. The results show that the ranging positioning algorithm that integrates the microwave ranging positioning system and the inertial navigation system can reduce the cumulative error of the system from the quadratic function of noise to the level of constant noise.

[0021] The present invention measures information based on a composite microwave antenna composed of microwave spin quantum sensors, collects and converges microwave signals propagating in free space into nanospace, and measures the range of microwave signals by detecting the state of local solid-state quantum probes, which can distinguish and detect well. By using quantum sensing and precision measurement, the system's measurement sensitivity to physical quantities is improved through characteristics such as quantum coherence and correlation. This method converts the detection of weak signals in free space into the detection of the interaction between electromagnetic fields and solid-state spins at the nanoscale, which improves the microwave signal measurement sensitivity of solid-state quantum sensors by 3-4 orders of magnitude. At the same time, by using multiple coherent interactions between solid-state spin quantum probes and microwave photons, quantum-enhanced position measurement accuracy is achieved, which can reach about one ten-thousandth of the wavelength, and is not easily affected by temperature, noise, weather, and humidity. The 10-micron level. Compared with traditional radar systems, this quantum measurement method does not require active devices such as amplifiers at the detection end, reduces the influence of factors such as electronic noise on the measurement limit, can adapt to harsh working environments, and has strong reliability. .

[0022] The present invention uses the output of the observation value of the sensor observation model as the input of the Kalman filter, and uses the state quantity to be estimated as the output. The Kalman filter is performed when there is a measurement value, and the result of the fused positioning trajectory is always near the real trajectory, which is also more robust and accurate than the positioning result based on ranging. The system state quantity at the next moment is estimated by the observation value at the previous moment, and the current state is updated progressively; data fusion is performed through the adaptive Kalman filter algorithm, which can not only eliminate the integral drift of the gyroscope well, but also significantly reduce the noise influence of the data, making the estimated attitude angle smoother. In this process, there is no need to perform derivative and quadratic derivative operations, which greatly reduces the calculation complexity. In addition, the amount of calculation can be reduced, the measurement accuracy of the system can be improved, and the shortcomings of the traditional extended Kalman filter, such as large amount of calculation and the inability to ensure that the white noise condition is established at all times, can be avoided.

[0023] The present invention uses the position result output by the ranging positioning algorithm to fuse, gradually pulls the result of inertial navigation to the vicinity of ranging positioning, corrects the cumulative errors of displacement and direction of continuous trajectory generated by inertial navigation, adopts error compensation algorithm to compensate the random error of the system, corrects the error of the inertial navigation system, updates and corrects the covariance according to the observation matrix and Kalman gain, and realizes the intelligent autonomous control positioning of the system. Not only is the fusion effect good, the calculation complexity is low, the speed is fast, there is high positioning accuracy and timely transmission, and the satellite positioning technology can complement the inertial navigation system under the condition of high noise of satellite signals and partial signal loss. The experimental results were analyzed by designing three experimental scenarios to verify the feasibility and reliability of the inertial navigation back-end algorithm. And based on the trajectory back-end optimization of the inertial navigation sensor, the accuracy and robustness of positioning are greatly improved, and the calculation complexity is greatly reduced compared with the general back-end optimization, which can be conveniently used in the positioning and navigation of micro inertial navigation. BRIEF DESCRIPTION OF THE DRAWINGS

[0024] The present invention is further described below in conjunction with the accompanying drawings and embodiments.

[0025] Figure 1 It is a flow chart of autonomous positioning of the unmanned system based on inertial navigation fused with microwave ranging of the present invention;

[0026] Figure 2 It is a simplified structure of the federated filter for INS / microwave ranging and positioning system fusion navigation. DETAILED DESCRIPTION

[0027] See also Figure 1 . According to the present invention, in an unmanned system, a Kalman filter is tightly combined with an inertial navigation system (INS) and a microwave ranging and positioning system, and is regarded as a navigation and positioning system. The result based on microwave ranging and positioning is integrated with the inertial navigation. The motion trajectory of the target carrier is inferred by a ranging and positioning algorithm that integrates microwave radar and the inertial navigation system in the form of pseudocode. The result of integrating the inertial navigation and positioning trajectory is always near the real trajectory. The motion process is modeled, and a sensor observation model that integrates two independent inertial navigation systems of the inertial micro-guided navigation system is constructed in the navigation and positioning system. The output of the observation value of the sensor observation model is used as the input of the Kalman filter, and the state quantity that needs to be estimated is used as the output;

[0028] The composite microwave antenna composed of microwave spin quantum sensors measures information, collects and converges microwave signals propagating in free space into nanospace, measures the range of microwave signals by detecting the state of local solid-state quantum probes, and uses the Doppler effect principle to detect the speed and depth information of the target, converts the detection of weak signals in free space into the detection of the interaction between electromagnetic fields and solid-state spins at the nanoscale, detects the interference between the microwave signal reflected by the object and the reference signal through solid-state spins, mixes each reflected microwave signal and performs FFT to obtain the phase of the microwave signal reflected by the object, as well as the position information and transmission distance of the object, and completes microwave ranging. At the same time, multiple coherent interactions between solid-state spin quantum probes and microwave photons are used to achieve quantum-enhanced position measurement accuracy.

[0029] The acceleration and angular velocity data collected by the micro-inertial navigation system are extracted to calculate the trajectory and position of the target vehicle. Based on the system noise and observation noise, the system state at the next moment is estimated through the observation value at the previous moment, and the current state is updated progressively.

[0030] In the process of digital signal processing, the time signal is converted into a spectrum through fast FFT transformation, which forms the basis of distance calculation. Then, the external acceleration is inferred by calculating the object position distance / height, and the position results output by the ranging positioning algorithm are integrated to gradually pull the results of inertial navigation to the vicinity of ranging positioning, correct the cumulative errors in displacement and direction of the continuous trajectory generated by inertial navigation, use the error compensation algorithm to compensate for the random error of the system, correct the error of the inertial navigation system, and update and correct the covariance according to the observation matrix and Kalman gain to realize the intelligent autonomous control positioning of the system.

[0031] Furthermore, it is necessary to construct a sensor observation model that integrates the two systems of the inertial navigation system (INS) and the microwave ranging positioning system. The state at the current moment is only related to the current sensor value and the initial value. The back-end optimization considers the state estimation problem over a longer period of time. Updating the current state is called progressive; in the front end of the inertial navigation, the motion information between the two states of the initial position and velocity is obtained, and the position information is calculated based on this. After that, the inertial navigation system continuously updates the current position and velocity by integrating and calculating the information of the motion sensor. Then, based on the inertial navigation track calculation based on the Kalman filter, the trajectory back-end of the information measured by the sensor is progressively optimized. Its advantage is that after the initial conditions are given, the current position, direction and velocity can be determined without external reference. By detecting the acceleration and angular velocity of the system, the inertial navigation system can detect position changes (such as movement to the east or west), velocity changes (speed magnitude or direction) and attitude changes (rotation around each axis).

[0032] The trajectory backend optimization takes the state that requires the minimum error function as the optimization target, constructs the error function fm(x) through the Euclidean distance, randomly selects a small amount δ as the zero bias adjustment, and superimposes the bias δ ​​on each state quantity, iteratively minimizes the error, and if the error function becomes larger after the first superposition of the bias, the bias is reversed, and the bias is repeatedly superimposed until the error function fmu(x) no longer continues to decrease. However, each time only a small adjustment δ is added to the state. In this process, the dimension of the calculation will not increase, only the number of state quantities will increase, and there is no need to perform derivative and quadratic derivative operations, which greatly reduces the computational complexity.

[0033] The sensor observation model is divided into two parts: prediction and update. The observation quantity is defined, the observation matrix is ​​constructed, and the future state is predicted through the observation data and the micro-inertial navigation system model. The state and control quantity are predicted through function mapping to predict the new state. The observation data is used for estimation, the filtered state estimation is performed, the state is updated, and corrections are made based on the observation data.

[0034] The sensor observation model infers the navigation system velocity through the observed quantity, measures the decomposition result of the velocity in the navigation system, converts the velocity into the navigation coordinate system, and obtains the observation matrix H. With the observation matrix and observation noise, we can start calculating the Kalman gain matrix K and updating the covariance matrix P, calculate the error state according to the filter gain matrix K, and finally update the state according to the estimated result of the error state.

[0035] The sensor observation model describes the probability distribution of sensor measurement values ​​in the sensor value space. The Kalman filter is used for posture estimation to update and correct the predicted state quantity and error. At the same time, the covariance is updated and corrected according to the linearized observation matrix and Kalman gain. The Kalman filter corrects the value covariance matrix.

[0036] Since the observation data includes the influence of noise and interference in the micro-inertial navigation system, the optimal estimation can also be regarded as a filtering process. First, the two independent subsystems of the inertial navigation system (INS) and the microwave ranging positioning system provide their respective system estimated states and covariances to the federated filter composed of the unscented Kalman filter through two independent Kalman filters; then, the unscented Kalman filter performs time update and optimal fusion to obtain the output state X and covariance P, and feeds them back to the Kalman filter for the next filtering iteration.

[0037] Next, the micro-INS starts to operate. The micro-INS reads the data in the inertial sensor and calculates the position, speed, and attitude information according to the kinematic formula. The Kalman filter estimates the position, speed, and direction of the drone based on the sensor readings and the algorithm for estimating the state of the micro-INS. The interference that is not tracked is treated as covariance noise. The inaccurate sensor measurement results are combined with motion prediction to obtain a filtered position estimate, and then the Kalman filter gain is calculated to obtain the autocorrelation matrix of the state prediction error. When the prior information of the dynamic system is unknown, the Kalman filter initializes the system state and covariance, calculates the micro-INS noise covariance and observation noise covariance using a small data window, updates the covariance according to the kinematic formula, and then outputs the corrected position and enters the next cycle. The cycle continues until the navigation demand is stopped. The filtered position estimate is better than all estimates from sensor readings or motion predictions alone.

[0038] The inertial navigation system includes at least a computer and a platform or module containing accelerometers, gyroscopes or other motion sensors. The gyroscopes and accelerometers provide motion data to the inertial reference assembly IR processor. The atmospheric data reference assembly ADR provides altitude, altitude rate and true airspeed to the IR processor. The atmospheric data reference assembly ADR receives pitch, roll, vertical velocity and acceleration data from the IR, measures the acceleration and angular velocity of the object, and uses these IR data to calculate thrust and ground effect compensation values. Then, ADR uses the calculated thrust and ground effect compensation values ​​as part of its static pressure source error correction calculation, and uses a computer to continuously estimate the position, attitude and velocity of the moving object. The IR has three accelerometers and three laser gyroscopes that can sense motion and angular motion. The three accelerometers are located on the X-axis, Y-axis and Z-axis of the aircraft to measure acceleration information on the three axes. Three ring laser gyros, each corresponding to a rotation axis (X, Y, Z), provide internal rotation data. IR uses an accelerometer and a gyroscope to measure the acceleration and angular velocity of the aircraft. The velocity is integrated twice, and the angular velocity is integrated to obtain the angular attitude. The acceleration in the carrier coordinate system is converted to the navigation coordinate system (the movement of the object observed from the ground) through the above integration. The initial coordinates are added to the integrated displacement to obtain the new coordinates, and the initial angle is added to the integrated angular displacement to obtain the new angle, thereby obtaining the current real-time position information through the initial position information.

[0039] The inertial navigation system uses the complementary characteristics of the gyroscope and accelerometer in the frequency domain, adopts the complementary filtering algorithm, fuses the data of the gyroscope and accelerometer, and establishes the unscented Kalman filtering algorithm suitable for nonlinear systems. The nonlinear function of the nonlinear system is linearized through methods such as Taylor series, and the high-order terms are omitted to obtain a linear system model. This method approximates the probability density of the nonlinear function. The unscented Kalman filtering method does not need to approximate the nonlinear function. Compared with the extended Kalman filter, the statistics of the unscented Kalman filter have not only higher accuracy, but also higher stability.

[0040] The Kalman filter is based on the posterior filter density at the previous moment and combines the state transition probability to solve the prior filter probability density at the current moment; the observation probability is obtained using the prior filter probability density and the observation likelihood density, and the prior filter probability density at this moment is used to solve the posterior filter probability density at this moment in combination with the observation likelihood probability density, giving a step-by-step recursive form of the posterior filter probability density.

[0041] After the Kalman filter obtains the posterior probability, the prior knowledge and actual observation values ​​are used to construct the posterior probability of the micro-inertial navigation system to estimate the state of the micro-inertial navigation system. The prior filtering probability density is obtained through the prediction step, and then the prior probability is corrected by the observation value to complete the edge probability density and obtain the posterior filtering probability density. According to the prior probability density and the observation value, the posterior filtering probability density is solved in combination with the observation likelihood probability density, and the prior filtering probability density, posterior probability density and observation likelihood density of the micro-inertial navigation system are calculated to obtain the observation probability.

[0042] When each new observation Z arrives, the posterior filter density at time k is obtained, and then the state value at time k is obtained. The prior probability density obtained by the state equation is corrected using the observation equation to obtain the posterior probability density at time k = 1, and then the posterior probability density at time k = 1 is recursively used as the prior probability density at time k = 2, and then the prior probability is corrected using the observation value, and the current state is optimized using the observation sequence. Due to the correction, the posterior probability density is closer to the true probability density of the estimated value than the prior probability density.

[0043] In Kalman filtering, the first step is that the inertial system display unit ISDU or the flight management computer FMC provides initial position data to the inertial reference unit IR processor, the mode selection unit MSU is used to select the operation mode, and the micro-inertial navigation system model is established according to the observation equation and the state equation, and the micro-inertial navigation system modeling is initialized. These equations need to be established according to specific problems; the second step is to predict the future state based on the micro-inertial navigation system model and the current state. The state and control quantity are predicted through function mapping. The prediction result is a state estimate value and a state covariance matrix used to describe the accuracy of the estimate value;

[0044] The third step is to update the predicted state quantity through the observation quantity, and update and modify the covariance according to the linearized observation matrix and Kalman gain. The updated result is used to describe the new state estimate and a new state covariance matrix of the accuracy of the corrected estimate.

[0045] The fourth step is to use the iterative method to directly use the observed data for calculation, and then derive the Kalman filter algorithm. The Kalman filter is iterated to obtain the estimate of the original micro-INS state vector. During the iteration process, each iteration will be predicted and updated. The prediction uses the last state estimate and covariance matrix, and the update uses the current observation data and predicted results. The number of iterations depends on the specific problem and the convergence speed of the algorithm;

[0046] The fifth step is that the Kalman filter outputs a state estimation sequence that describes the process of the micro-inertial navigation system state changing over time. This sequence is used to predict future states or to control the micro-inertial navigation system.

[0047] The above: state represents the minimum set of variables of a dynamic micro-inertial navigation system, which is called state variables. State variables are a set of variables describing the change of the state of the micro-inertial navigation system over time, or variables used to describe the change of a state in the network over time, which are called state variables; state vectors are k state variables that can fully describe the behavior of a system, which can be regarded as the coordinates of each component of the vector input, called state vectors; state equations are a set of independent first-order differential equations represented by state variables and excitations, or first-order differential equations that describe the relationship between the first-order derivatives of the system state variables and the state variables and excitations, called state equations;

[0048] State space: the space where the state vector in (t) is located. State trajectory: the path traced by the state vector endpoints changing with time in the state space is called the state trajectory:

[0049] The state equation describes how the state of the system changes over time, and the observation equation describes Kalman: Use the linear system state equation to optimally estimate the system state through the system input and output observation data, and obtain observation data from the micro-inertial navigation system state. Use the first group of observations in the group observation data..., and then establish a mathematical model by differential equations. After modeling, the result data is restored by "inverse generation" to obtain measurement: refers to the observation information about the target state contaminated by noise and the information extracted from the observation signal, including: direct position estimation, ranging, azimuth information, arrival time difference between sensors, etc.

[0050] The output signal is a function of the state variable and the input signal:

[0051] Signal is the carrier of information - information is transformed, encoded and modulated to produce a signal that carries the information. The signal usually received is called the "observation signal".

[0052] The measurement signal that the information fusion process focuses on is usually not the original observation signal, but the output signal processed by the sensor system or the detection system. The premise of information processing and information fusion is to obtain information.

[0053] The microwave spin quantum sensor contains a diamond chip of the NV sensor spin layer connected to a thin film magnet, forming a microwave spin quantum sensor based on broadband spin. The microwave spin quantum sensor and the metal nanostructure form a composite microwave antenna, which collects and converges microwave signals propagating in free space into nanospace, thereby measuring the microwave signal by detecting the local state of the solid-state quantum probe, and obtaining the phase of the microwave signal reflected by the object and the position information of the object through the interference result of the solid-state spin detection object and the reference signal. This method converts the detection of weak signals in free space into the detection of the interaction between electromagnetic fields and solid-state spins at the nanoscale, improving the microwave signal measurement sensitivity of the solid-state quantum sensor by 3-4 orders of magnitude. This method uses multiple coherent interactions between solid-state spin quantum probes and microwave photons to achieve quantum-enhanced position measurement accuracy of 10 microns (about one ten-thousandth of the wavelength), achieving 10 -4 Wavelength-precision positioning. Compared with traditional radar systems, this quantum measurement method does not require active devices such as amplifiers at the detection end, reducing the impact of factors such as electronic noise on the measurement limit. It can also further improve the radio positioning accuracy and sampling rate based on solid-state spin quantum sensing, and the positioning technology exceeds the performance level of existing radars.

Claims

1. An autonomous positioning method for an unmanned system based on the fusion of microwave ranging and inertial navigation, having the following technical features: In unmanned systems, the Kalman filter is tightly combined with the inertial navigation system (INS) and the microwave ranging positioning system, and regarded as a navigation and positioning system. The results based on microwave ranging positioning are integrated with the inertial navigation. In the form of pseudo code, the ranging positioning algorithm that integrates microwave radar and inertial navigation system is used to infer the motion trajectory of the target carrier. The result of integrating the inertial navigation positioning trajectory is always near the real trajectory. The motion process is modeled, and a sensor observation model that integrates two independent inertial navigation systems of inertial micro navigation in the navigation and positioning system is constructed. The output of the observation value of the sensor observation model is used as the input of the Kalman filter, and the state quantity that needs to be estimated is used as the output. The composite microwave antenna composed of microwave spin quantum sensors measures information, collects and converges microwave signals propagating in free space into nanospace, measures the range of microwave signals by detecting the state of local solid-state quantum probes, and uses the Doppler effect principle to detect the speed and depth information of the target, converts the detection of weak signals in free space into the detection of the interaction between electromagnetic fields and solid-state spins at the nanoscale, detects the interference between the microwave signal reflected by the object and the reference signal through solid-state spins, mixes each reflected microwave signal and performs FFT to obtain the phase of the microwave signal reflected by the object, as well as the position information and transmission distance of the object, and completes microwave ranging. At the same time, multiple coherent interactions between solid-state spin quantum probes and microwave photons are used to achieve quantum-enhanced position measurement accuracy. The acceleration and angular velocity data collected by the micro-inertial navigation system are extracted to calculate the trajectory and position of the target vehicle. Based on the system noise and observation noise, the system state at the next moment is estimated through the observation value at the previous moment, and the current state is updated progressively. In the process of digital signal processing, the time signal is converted into a spectrum through fast FFT transformation, which forms the basis of distance calculation. Then, the external acceleration is inferred by calculating the object position distance / height, and the position results output by the ranging positioning algorithm are integrated to gradually pull the results of inertial navigation to the vicinity of ranging positioning, correct the cumulative errors in displacement and direction of the continuous trajectory generated by inertial navigation, use the error compensation algorithm to compensate for the random error of the system, correct the error of the inertial navigation system, and update and correct the covariance according to the observation matrix and Kalman gain to realize the intelligent autonomous control positioning of the system.

2. The autonomous positioning method for an unmanned system based on the fusion of microwave ranging and inertial navigation as claimed in claim 1, characterized in that: In the front end of the inertial navigation, the motion information between the initial position and speed states is obtained, and the position information is calculated based on this. After that, the inertial navigation system continuously updates the current position and speed by integrating and calculating the information from the motion sensor. Then, the inertial navigation track is calculated based on the Kalman filter, and the trajectory of the sensor measured information is gradually optimized in the back end.

3. The autonomous positioning method for an unmanned system based on the fusion of microwave ranging and inertial navigation as claimed in claim 2, characterized in that: The trajectory backend optimization takes the state that requires the minimum error function as the optimization target, constructs the error function fm(x) through the Euclidean distance, randomly selects a small amount δ as the adjustment amount of the zero bias, superimposes the bias δ ​​on each state quantity, and iteratively minimizes the error. If the error function becomes larger after the first superposition of the bias, the bias is reversed, and the bias is repeatedly superimposed until the error function fmu(x) no longer continues to decrease.

4. The autonomous positioning method for an unmanned system based on the fusion of microwave ranging and inertial navigation as claimed in claim 1, characterized in that: The sensor observation model is divided into two parts: prediction and update. The observation quantity is defined, the observation matrix is ​​constructed, and the future state is predicted through the observation data and the micro-inertial navigation system model. The state and control quantity are predicted through function mapping to predict the new state. The filtered state estimation is performed and the state is updated through the observation data, and corrections are made according to the observation data.

5. The autonomous positioning method for an unmanned system based on the fusion of microwave ranging and inertial navigation as claimed in claim 4, characterized in that: The sensor observation model infers the navigation system velocity through the observed quantity, measures the decomposition result of the velocity in the navigation system, converts the velocity into the navigation coordinate system, and obtains the observation matrix H. With the observation matrix and observation noise, we can start calculating the Kalman gain matrix K and updating the covariance matrix P, calculate the error state according to the filter gain matrix K, and finally update the state according to the estimated result of the error state.

6. The autonomous positioning method for an unmanned system based on the fusion of microwave ranging and inertial navigation as claimed in claim 1, characterized in that: The sensor observation model describes the probability distribution of sensor measurement values ​​in the sensor value space, uses Kalman filtering to perform attitude estimation, updates and corrects the predicted state quantity and errors, and updates and corrects the covariance according to the linearized observation matrix and Kalman gain. The covariance matrix is ​​the value corrected by Kalman filtering.

7. The autonomous positioning method for an unmanned system based on the fusion of microwave ranging and inertial navigation as claimed in claim 1, characterized in that: The two independent subsystems of inertial navigation system (INS) and microwave ranging positioning system provide their own estimated states and covariances to the federated filter composed of unscented Kalman filter through two independent Kalman filters; then, the unscented Kalman filter performs time update and optimal fusion to obtain the output state X and covariance P, and feeds them back to the Kalman filter for the next filtering iteration.

8. The autonomous positioning method for an unmanned system based on the fusion of microwave ranging and inertial navigation as claimed in claim 1, characterized in that: The micro-inertial navigation system reads the data in the inertial sensor and calculates the position, speed and attitude information according to the kinematic formula; the Kalman filter estimates the position, speed and direction of the drone based on the sensor readings and the algorithm for estimating the state of the micro-inertial navigation system, treats the interference that is not tracked as covariance noise, combines the inaccurate sensor measurement results with the motion prediction, obtains a screened position estimate, and then calculates the Kalman filter gain to obtain the autocorrelation matrix of the state prediction error; when the prior information of the dynamic system is unknown, the Kalman filter initializes the system state and covariance, uses a small data window to calculate the micro-inertial navigation system noise covariance and the observation noise covariance, updates the covariance according to the kinematic formula, and then outputs the corrected position and enters the next cycle, which is repeated until the navigation demand is stopped.

9. The autonomous positioning method for an unmanned system based on the fusion of microwave ranging and inertial navigation as claimed in claim 1, characterized in that: The Kalman filter is based on the posterior filter density at the previous moment and combines the state transition probability to solve the prior filter probability density at the current moment; the observation probability is obtained using the prior filter probability density and the observation likelihood density, and the prior filter probability density at this moment is used to solve the posterior filter probability density at this moment in combination with the observation likelihood probability density, giving a step-by-step recursive form of the posterior filter probability density.

10. The autonomous positioning method for an unmanned system based on the fusion of microwave ranging and inertial navigation as claimed in claim 9, characterized in that: After the Kalman filter obtains the posterior probability, the prior knowledge and actual observation values ​​are used to construct the posterior probability of the micro-inertial navigation system to estimate the state of the micro-inertial navigation system. The prior filtering probability density is obtained through the prediction step, and then the prior probability is corrected by the observation value to complete the edge probability density and obtain the posterior filtering probability density. According to the prior probability density and the observation value, the posterior filtering probability density is solved in combination with the observation likelihood probability density, and the prior filtering probability density, posterior probability density and observation likelihood density of the micro-inertial navigation system are calculated to obtain the observation probability.

Images