Multi-source heterogeneous navigation source error model optimization method and simulation system

Abstract

The invention discloses a multi-source heterogeneous navigation source error model optimization method and a simulation system, which are applied to an unmanned equipment navigation system, and the method comprises the following steps: S1, calculating an error model of each sensor sub-system; s2, initial calibration; s3, primarily screening the sensor subsystem; s4, secondary screening of the sensors; s5, simulating motion parameters; s6, judging whether the optimized sensor is greater than or equal to 2 or not; if yes, simulation analysis and Kalman filtering correction are carried out, and simulation data are stored; if not, directly storing the prevention data; s7, judging whether simulation is finished or not, if so, entering the step S8, and if not, returning to the step S3; and S8, drawing a curve. The simulation model can be compatible with various information sources at the same time, corresponding navigation models are constructed for different information sources, model selection can be flexibly carried out according to the information sources, different matrix types can be flexibly selected according to different actual use environments, and the simulation model is higher in compatibility, wider in coverage, higher in flexibility and better in environmental adaptability.

Description

Technical Field

[0001] The invention relates to unmanned technology and navigation technology, in particular to a multi-source heterogeneous navigation source error model optimization method and a simulation system. Background Art

[0002] With the development of the times, unmanned technology has become the mainstream development trend. Unmanned vehicles and drones are developing rapidly, and higher requirements are also put forward for positioning and navigation technology. At present, UWB, LTE, Bluetooth, WIFI and other technologies are mainly used for indoor positioning; navigation, inertial navigation, pseudo-satellite navigation, etc. are mainly used for outdoor positioning, and UWB, LTE, Bluetooth, WIFI and other technologies are mainly used for indoor positioning.

[0003] In recent years, unmanned technology has rapidly emerged, and location services have become widely popular, which has also put forward higher requirements for reliable navigation systems. The Global Navigation Satellite System (GNSS) is a common outdoor positioning method, but GNSS has extremely high requirements for the environment. As a result, in complex environments, when GNSS satellite signals are blocked by obstructions, signal strength is weakened, noise increases, accuracy decreases, and data is lost, making GNSS unable to provide precise positioning. At this time, the availability, continuity and reliability of GNSS PNT services cannot be guaranteed.

[0004] Since the satellite navigation system in the commonly used navigation technology is easily interfered and cannot be used in indoor environments, combined navigation or micro PNT and other methods are mainly used for positioning and navigation. However, even so, navigation will inevitably have errors caused by various reasons. Most of the current error analysis models for different navigation methods are not comprehensive enough and can only support error analysis of 2~3 navigation methods. They cannot be effectively compatible with other navigation methods. In addition, the navigation error model is relatively fixed and cannot be selected according to environmental factors, which is not flexible enough. The current PNT error analysis model is not flexible and rich enough. Most of them are only applicable to 1-3 navigation methods, such as the combined navigation system of the global satellite positioning system and the inertial navigation system, the combined navigation system based on sensor information fusion, the GPS combined navigation system, and the Doppler combined navigation system. Its mode is fixed and the coordination mode cannot be changed according to the environment.

[0005] The existing problems mainly focus on the following three points: 1. The model is single, has a small scope of application, and has poor compatibility. Once the navigation method is changed, the error model needs to be rebuilt, which is time-consuming and labor-intensive, and increases development costs.

[0006] 2. The environmental adaptability is poor, and the error model analysis can only be performed on fixed environmental parameters. Once the environment changes, the accuracy will be seriously affected, resulting in unstable and inaccurate error model results, and the results have no reference value.

[0007] 3. Low flexibility, unable to change the error fusion model according to actual needs. Summary of the invention

[0008] In view of the problems existing in the prior art, the purpose of the present invention is to provide a multi-source heterogeneous navigation source error model optimization method and simulation system. The simulation model can be compatible with multiple information sources at the same time, and corresponding navigation models are constructed for different information sources. The model can be flexibly selected according to the information source, and different matrix types can be flexibly selected according to the actual usage environment. The system has stronger compatibility, wider coverage, higher flexibility, better environmental adaptability and powerful functions.

[0009] To achieve the above object, the present invention provides a multi-source heterogeneous navigation source error model optimization method, which is applied to an unmanned equipment navigation system, wherein the multi-source heterogeneous navigation sources include: inertial information output by an inertial navigation subsystem, longitude and latitude altitude information output by a satellite navigation subsystem, a visual odometer scoring system, a laser radar odometer scoring system, relative measurement navigation information output by a UWB subsystem, and heading angle information output by a magnetometer scoring system; the method comprises the following steps: S1. Calculate the error model of each sensor subsystem; S2. Initial calibration of each sensor; S3. Preliminary screening of sensor subsystems; calculate the error covariance of the above six subsystems respectively, and PNT sources with error covariance greater than the error threshold value are not considered; only other subsystem models that meet the error threshold value requirements can enter the next optimization process; S4. Secondary screening of sensors: Draw performance radar charts for the other subsystem models that meet the threshold requirements according to the five aspects of Class II indicators, calculate the area within each performance radar chart, and sort them from large to small according to the area, and select the top three PNT sources as the final optimization result. If there are less than three, keep the top two; S5. Simulate motion parameters: simulate the motion trajectory of the unmanned platform and its corresponding parameters through the trajectory generator; S6. Determine whether the number of sensors after the optimization is greater than or equal to 2; if so, perform simulation analysis, Kalman filter correction, and save simulation data; if not, directly save the prevention data; S7. Determine whether the simulation is finished, if so, proceed to step S8, if not, return to step S3; S8. Draw a curve of the fusion error changing over time.

[0010] Further, in step S1, the sensor subsystem error model includes a satellite navigation subsystem error model, an inertial navigation subsystem error model, a lidar subsystem error model, a visual navigation subsystem error model, a magnetometer subsystem error model and a UWB ranging subsystem error model.

[0011] Furthermore, in the satellite navigation subsystem error model, the error model is: in, They represent the factors of navigation message error, satellite clock error, ionospheric delay, tropospheric delay, etc. Satellite pseudo-ranging error, represents the measurement noise, It represents the error caused by multipath effect to pseudo-range measurement, represents the satellite navigation system error, represents the pseudorange, represents the receiver clock error, Represents the error caused by multipath.

[0012] Furthermore, in the inertial navigation subsystem error model, the output relationship of the gyro component of the inertial navigation is expressed as: Among them, the actual angular velocity output of the gyro component The scale factor error is included in , coupling force, is the angular velocity, is the sensitivity and sensitivity coefficient Product, zero drift and random colored noise ; There is colored noise Modeled as a random constant and a first-order Markov process, as follows: The output relationship of the accelerometer component of inertial navigation can generally be expressed as: Actual acceleration output of the acceleration component The same model is , is a constant drift, is random colored noise.

[0013] Furthermore, in the laser radar subsystem error model, the laser radar uses the time difference method to obtain distance information. The transmitting part transmits a laser beam to the target object. When the laser beam reaches the surface of the target object, it is reflected and received by the receiving part. The relative distance between the target point and the laser radar is measured by the time difference between the two pulses transmitted and received. The specific formula is as follows:

[0014] c represents the speed of light, t represents time, and the scanning point position information is in spherical coordinates (r, , ) to record, according to the scanning point distance r, pitch angle and azimuth It can be converted into the rectangular coordinate system of the laser radar carrier itself.

[0015] Furthermore, in the visual navigation subsystem error model, the relative position of visual measurement is modeled as: is the relative position output by the visual simulator in two adjacent frames. is the relative position truth value, is Gaussian white noise.

[0016] Furthermore, in the magnetometer subsystem error model, the three-axis components of the vector H measured by the magnetometer in the geographic coordinate system are: , and ;in , The combined force It always refers to the north direction, and D is the magnetic heading, which can be expressed as: Since the value range of the magnetic heading angle is , and the range of the experimentally calculated inverse tangent function is ,and , in order to eliminate singular values, the calculation of magnetic heading is constrained as follows: The magnetometer is affected by its own errors and external magnetic field interference factors, and its measurement model can be established as follows:

[0017] is the sensitivity error matrix, is the non-orthogonal error, is the soft magnetic interference matrix, is the hard magnetic interference vector, is the magnetometer bias, is the Gaussian noise of the sensor, The true magnetic field vector value, Indicates the measured magnetic field vector value.

[0018] Furthermore, in the UWB ranging subsystem error model, two adjacent unmanned vehicles in the cluster can assist in positioning by receiving the collaborative information of adjacent unmanned vehicles within the ranging range. Under the condition of good line of sight, the inter-machine ranging value based on UWB can be modeled as: is the real distance measurement between machines. is the distance information output by the sensor, represents noise; if the current position is , the position of the i-th adjacent node is , then the UWB output is: The position measurement value is , the position measurement value of the adjacent node of the i-th vehicle is , Taylor expansion of the above formula is: In the formula is the position of the adjacent node, Indicates noise; Error It can be modeled as: Further simplification yields: UWB error between nodes middle The part is the equivalent ranging noise that combines the position error of node i and the UWB ranging noise, and the noise variance is: In the formula is the position covariance of the ith node.

[0019] Further, the specific implementation of step S3 is as follows: S3.1 calculation sets the Class I indicator as error covariance; the error covariance is solved by the sensor subsystem error model part; S3.2 Setting the error covariance threshold value: Determine the error covariance threshold value based on the complexity of the specific environment and specific needs; the threshold values ​​of different indicators can be set in the radar chart according to the actual situation; S3.3 Preliminary screening: Calculate the error covariance of the above six subsystems respectively. PNT sources with error covariance greater than the error threshold value will not be considered; PNT source subsystem models with error covariance less than the error threshold value will enter the next optimization process; The specific implementation of step S4 is as follows: S4.1 Setting Type II indicators: Error covariance, error controllability , error influencing factors, error propagation mechanism and positioning continuity Indicators are set in five aspects and evaluated through radar charts; S4.2 Setting the threshold value of Class II indicators: Determine the threshold value of Class II indicators based on the complexity of the specific environment and specific needs; S4.3 Secondary screening: Draw the performance radar charts of the other subsystem models that meet the threshold requirements according to the five aspects of Class II indicators, calculate the area within each performance radar chart, and sort them from large to small according to the area, and select the top three PNT sources as the final preferred results. If there are less than three, retain the top two.

[0020] On the other hand, the present invention provides a multi-source heterogeneous navigation source error model simulation system, which is used to implement the multi-source heterogeneous navigation source error model optimization method according to the present invention. The system includes six types of sensor subsystem error models, error fusion transmission models, selection models and fusion simulation models.

[0021] Beneficial effects: The error simulation system of the present invention can support 6 PNT information sources, including laser radar, satellite navigation, mobile communication, inertial navigation, UWB, vision, magnetometer, gravity field, etc. Compared with the traditional navigation error simulation model, the error simulation model of the present invention is richer, more convenient to use, and simpler to operate. In addition, the present invention designs a "matrix selection" process to integrate these subsystems and perform optimal combination. According to the environmental information and sensor conditions at the time, the optimal combination is selected to minimize the error.

[0022] This method supports error analysis of up to six navigation methods, almost covering several current mainstream navigation methods. It does not need to change the model due to different navigation methods, and has better compatibility and higher applicability.

[0023] This method can flexibly judge and allocate error test models according to different input navigation modes, thereby improving the model's adaptability.

[0024] This method can flexibly select the configuration error fusion model for navigation error analysis according to actual needs. BRIEF DESCRIPTION OF THE DRAWINGS

[0025] Figure 1 It shows a multi-source heterogeneous navigation source error model optimization method and a simulation system structure diagram according to the present invention; Figure 2 A schematic diagram of laser radar ranging according to the present invention is shown; Figure 3 A schematic diagram of the distribution of the earth's magnetic field lines according to the present invention is shown; Figure 4 A schematic diagram of the measurement principle of a three-axis magnetometer according to the present invention is shown; Figure 5A schematic diagram of a PNT service performance indicator radar according to a Class II indicator of the present invention is shown. DETAILED DESCRIPTION

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

[0027] In the description of the present invention, it should be noted that the terms "center", "upper", "lower", "left", "right", "vertical", "horizontal", "inner", "outer", etc., indicating the orientation or positional relationship, are based on the orientation or positional relationship shown in the drawings, and are only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as limiting the present invention. In addition, the terms "first", "second", and "third" are used for descriptive purposes only, and cannot be understood as indicating or implying relative importance.

[0028] In the description of the present invention, it should be noted that, unless otherwise clearly specified and limited, the terms "installed", "connected", and "connected" should be understood in a broad sense, for example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection or an electrical connection; it can be a direct connection, or it can be indirectly connected through an intermediate medium, or it can be the internal communication of two components. For ordinary technicians in this field, the specific meanings of the above terms in the present invention can be understood according to specific circumstances.

[0029] The following combination Figure 1-Figure 5 The specific embodiments of the present invention are described in detail. It should be understood that the specific embodiments described herein are only used to illustrate and explain the present invention, and are not used to limit the present invention.

[0030] According to the multi-source heterogeneous navigation source error model optimization method and simulation system of the present invention, multiple PNT information sources are supported, including: laser radar, satellite navigation, mobile communication, inertial navigation, UWB, vision, magnetometer, gravity field, etc., that is, the multi-source heterogeneous PNT sources available for simulation by the system include: inertial information output by the inertial navigation subsystem, longitude and latitude altitude information output by the satellite navigation subsystem, relative measurement navigation information output by the visual mileage scoring system, the laser radar mileage scoring system, and the UWB subsystem, and the heading angle information output by the magnetometer scoring system, a total of 6 information sources. Based on the above information sources, by constructing an error fusion transmission model, a selection model and a fusion simulation model, an error simulation system and method for multi-source heterogeneous PNT information sources are realized.

[0031] The multi-source heterogeneous navigation source error model simulation system according to the present invention comprises: six types of sensor subsystem error models, a subsystem error classification optimization model, and a multi-source heterogeneous PNT information source error simulation model.

[0032] Among them, the six-category sensor subsystem error model is used to construct an error model based on the error characteristics of each type of sensor. It is mainly used for the calculation of error covariance indicators in primary screening and secondary screening.

[0033] The subsystem error classification optimization model is used to screen the sensor combination that can be used for fusion solution in this environment.

[0034] The error simulation model of multi-source heterogeneous PNT information source is used to combine the selected sensors for fusion calculation. It is used to solve the fusion positioning error, so it is called the error simulation model of multi-source heterogeneous PNT information source.

[0035] like Figure 1 As shown, the multi-source heterogeneous navigation source error model optimization method according to the present invention comprises the following steps: S1. Setup: Calculate the error model of each sensor subsystem; calculate the positioning error covariance of each sensor.

[0036] S2. Initial calibration of each sensor.

[0037] S3. Initial screening of sensor subsystems; calculate the error covariance of the above six subsystems respectively, and PNT sources with error covariance greater than the error threshold value are not considered; only other subsystem models can enter the next optimization process.

[0038] S4. Secondary screening of sensors: Draw performance radar charts for the above six types of subsystem error models according to the five aspects of Class II indicators, calculate the area within each performance radar chart, and sort them from large to small according to the area, and preliminarily select the top four PNT sources as the final optimization results.

[0039] S5. Simulate motion parameters: Use the track generator to simulate the motion trajectory and corresponding parameters of the unmanned platform (such as unmanned vehicles, unmanned ships, and unmanned aerial vehicles). Simulating motion parameters is actually a simulation method.

[0040] S6. Determine whether the number of sensors after optimization is greater than or equal to 2; if so, perform simulation analysis, Kalman filter correction, and save simulation data; if not, directly save simulation data; Based on the above optimization results, obtain the error model parameters of the subsystem after optimization, and fuse the above subsystem error model through Kalman filtering to obtain the fused collaborative positioning error model. Generally, 3-4 optimal sensors will be retained, but if the accuracy is too poor during the initial screening, there may be less than 2 left in the end. If there are less than 2, fusion solution cannot be performed. Then there is no need for fusion simulation. So it is set to be greater than or equal to 2 here.

[0041] S7. Determine whether the simulation is finished. If so, go to step S8; if not, return to step S3.

[0042] S8. Draw a curve of the fusion error changing over time.

[0043] Specifically, the sensor subsystem error model includes a satellite navigation subsystem error model, an inertial navigation subsystem error model, a lidar subsystem error model, a visual navigation subsystem error model, a magnetometer subsystem error model and a UWB ranging subsystem error model, which are as follows: (1) Satellite navigation subsystem error model In satellite navigation systems, the sources of errors collected by receivers are mainly affected by many factors such as atmospheric refraction, building obstruction, and multipath effects.

[0044] Taking into account the external environmental factors such as the ionosphere, troposphere, multipath effect, geomagnetic interference, as well as the influence of satellite clock error and satellite ephemeris error, the commonly used satellite navigation system error model is: They represent the factors of navigation message error, satellite clock error, ionospheric delay, tropospheric delay, etc. Satellite pseudo-ranging error, represents the measurement noise. In a conventional satellite navigation receiver, the above factors will cause about 10 meters positioning error. It represents the error caused by multipath effect to pseudorange measurement. represents the satellite navigation system error, represents the pseudorange, represents the receiver clock error, The error caused by multipath is modeled in three states for satellite output, namely normal state, interfered state and rejected state. The normal state is modeled as Gaussian white noise. In the interfered state, the standard deviation coefficient of the model is increased; in the rejected state, the output is directly 0. Then, the error covariance of the satellite navigation subsystem error model can be solved by the following formula:

[0045] (2) Inertial navigation subsystem error model Since the inertial navigation system does not rely on any external information and does not radiate energy to the outside, it has good concealment and is not affected by external electromagnetic interference. It can work in the air or on the surface of the earth all the time, all the time, and in all weather conditions. The strapdown inertial navigation system has a simple structure, small size, light weight, low cost, simple maintenance, high reliability, and can also improve its fault tolerance through redundancy technology, which is suitable for unmanned vehicle clusters. Therefore, the high-precision navigation unmanned vehicles and low-precision navigation unmanned vehicles in the cluster of this project both use the strapdown inertial navigation system as the core to achieve navigation through information fusion.

[0046] The output relationship of the gyro component of inertial navigation can generally be expressed as: Actual angular velocity output of the gyro component The scale factor error is included in , coupling force, is the angular velocity, is the sensitivity and sensitivity coefficient Product, zero drift and random colored noise . Among them, colored noise Modeled as a random constant and a first-order Markov process ,Right now: The output relationship of the accelerometer component of inertial navigation can generally be expressed as: Actual acceleration output of the acceleration component The same model is , is a constant drift, It is random colored noise. Except for colored noise, other noises can be largely eliminated by the turntable. Therefore, this project is aimed at colored noise. Modeled as a first-order Markov process, the position output error can be modeled as:

[0047] Among them, L, and h represent longitude, latitude and altitude respectively, so, , and They represent their derivatives respectively, where , , represents the north velocity, east velocity and normal velocity, Indicates the zero drift error, is random white noise.

[0048] The present invention needs to consider the altitude information when designing the simulation. However, the altitude channel of the inertial navigation system is unstable, that is, it is easily affected by errors. The theoretical error model of the altitude channel based on the inertial navigation is: , e represents the natural logarithm, when >1, the height channel error will have an impact on the square of time or more, and will generally show a transmission trend. Therefore, when the unmanned vehicle simulated in this project is working at a long flight time, the pure inertial navigation height channel cannot be used alone for a long time, and other navigation information sources need to be introduced to correct the node height information. Then, the error covariance of the inertial navigation subsystem error model can be solved by the following formula: in, represents the error covariance of the inertial navigation subsystem error model, Express expectations, Represents longitude, latitude and altitude respectively.

[0049] (3) LiDAR subsystem error model Laser radar uses the time difference method to obtain distance information. The transmitting part emits a laser beam to the target object. When the laser beam reaches the surface of the target object, it is reflected and received by the receiving part. The relative distance between the target point and the laser radar is measured by the time difference between the two pulses transmitted and received. That is: Where c represents the speed of light, t represents the time scanning point position information in spherical coordinates (r, , ) to record, such as Figure 2 As shown, according to the scanning point distance r, pitch angle and azimuth It can be converted to the rectangular coordinate system of the laser radar carrier itself Distance Error , the pitch angle error is and the azimuth error is Based on the actual test data, for the laser radar ranging information, this part of the error simulation has Gaussian white noise, with a mean of 0 and a variance of 0.01m. The error output of the laser radar odometer uses the Gaussian Markov mathematical model.

[0050] Then, the error covariance of the laser radar subsystem error model can be solved by the following formula: (4) Error model of visual navigation subsystem The visual navigation method uses the camera to obtain the surrounding environment information and solves the camera's own movement from the observed environmental information. According to the principle of the visual perception module commonly used in the node: Point in camera coordinate system , and its corresponding image coordinates are , according to the triangle similarity relation: Where x, y represent the image coordinates, and f represents the focal length.

[0051] Assume the image is in pixel coordinate system Axis scaled times, in Axis scaled times, the origin is shifted ,but The coordinates are converted to pixel coordinates as follows: definition , , we can get: Where, the focal length is in meters, , The unit of is pixel / meter, so , , , The unit is pixel. The matrix form of the above formula is: After obtaining the above formula, it needs to be normalized to obtain the target point Projection on the camera normalized plane gives point The normalized coordinates of the camera. This coordinate is equivalent to the The plane at which the point is located is called the normalized plane. Relative to the optical center The sight angle and line of sight elevation They are: According to the principle of visual measurement, visual positioning is to solve the world coordinates from the pixel coordinates, so the positioning error of a certain point in space is the error of the world coordinates caused by the pixel coordinate error, and the world coordinate system can be converted into the camera coordinate system through the rotation and translation matrix, so as to obtain the relative position of the target relative to the camera. However, the fitting error of the image feature point projected by the target in the pixel coordinate system is random, so the error of the azimuth and pitch angle calculated at this point is random, which can generally be modeled as Gaussian white noise.

[0052] For the relative position of visual measurement, the present invention is modeled as:

[0053] is the relative position output by the visual simulator in two adjacent frames. is the relative position truth value, is Gaussian white noise. The position solution of the visual odometer uses the SLAM algorithm for position solution, and the error generated in the solution process is modeled as a first-order Markov, random number, and Gaussian white noise accumulation method. Then, the error covariance of the visual navigation subsystem error can be solved by the following formula:

[0054] (5) Magnetometer scoring system error model The actual distribution of magnetic field lines generated by the Earth's magnetic field is as follows: Figure 3 As shown. Magnetic lines of force point from the geomagnetic south pole to the geomagnetic north pole inside the earth, and from the geomagnetic north pole to the geomagnetic south pole outside the earth. The distribution characteristics of magnetic lines of force near the earth are that the direction of the magnetic lines of force near the equator is horizontal to the surface of the earth, and the magnetic field is the weakest, while the direction of the magnetic lines of force near the two magnetic poles is almost vertical to the surface of the earth, and the magnetic field is the strongest. The geomagnetic south and north poles are not located on the earth's rotation axis, and the geomagnetic poles do not coincide with the geographic north and south poles.

[0055] Magnetometers can measure magnetic field vectors at any location on the earth. This project uses it as a navigation information source that can be used by unmanned clusters to solve the heading angle information of unmanned vehicles. The method by which magnetometers calculate the yaw angle is to measure the direction and strength of the earth's magnetic field and use the magnetic field data to calculate the yaw angle of the unmanned vehicle. Figure 4 As shown, the three-axis components of the vector H measured by the magnetometer in the geographic coordinate system (east-north-sky) are: , and .in , The combined force It always refers to the north direction, and D is the magnetic heading, which can be expressed as: Since the value range of the magnetic heading angle is , and the range of the experimentally calculated inverse tangent function is ,and , in order to eliminate singular values, the present invention constrains the calculation of magnetic heading, namely: The magnetometer is affected by its own errors and external magnetic field interference factors, and its measurement model can be established as follows:

[0056] is the sensitivity error matrix, is the non-orthogonal error, is the soft magnetic interference matrix, is the hard magnetic interference vector, is the magnetometer bias, is the Gaussian noise of the sensor, The true magnetic field vector value, Indicates the measured magnetic field vector value.

[0057] In addition to the output error caused by the non-orthogonality of the three axes of the sensor, the sensitivity error and the zero offset error will also affect the output of the sensor. Under the influence of the non-orthogonality of the three axes, the sensitivity error and the zero offset error, the output of the sensor can be expressed as:

[0058] in, is the sensitivity matrix; is the transformation matrix from orthogonal to non-orthogonal; is the zero point offset error matrix; It is the ideal output value of the sensor when the three axes are orthogonal; is the actual output value of the sensor. Substituting the parameters into the formula, we can get:

[0059] The actual calculated value of the sensor is obtained, which includes the zero drift error, three-axis sensitivity error and three-axis non-orthogonality error. Then, the error covariance of the magnetometer subsystem error model can be solved by the following formula:

[0060] in, represents the error covariance of the magnetometer system error model, Indicates the average value of sensor output measured multiple times.

[0061] (6) UWB ranging subsystem error model

[0062] UWB signals have the advantages of high data transmission rate, strong penetration, strong anti-interference ability, and high multipath resolution, and are often used for inter-machine ranging. The present invention uses UWB ranging information to achieve collaborative navigation within an unmanned cluster. The ranging error of UWB mainly comes from factors such as clock error and antenna delay. Two adjacent unmanned vehicles in a cluster can assist in positioning by receiving collaborative information from adjacent unmanned vehicles within the ranging range. Under good line of sight conditions, the inter-machine ranging value based on UWB can be modeled as: is the real distance measurement between machines. is the distance information output by the sensor, Indicates noise. If the current position is , the position of the adjacent node of the i-th vehicle is , then the UWB output is: The position measurement value is , the position measurement value of the adjacent node of the i-th vehicle is , Taylor expansion of the above formula is: In the formula is the position of the adjacent unmanned vehicle, Indicates noise. Error It can be modeled as: Further simplification yields: UWB error between nodes middle The part is the equivalent ranging noise that combines the position error of node i and the UWB ranging noise, and the noise variance is: In the formula is the position covariance of the i-th node. Then, the error covariance of the UWB ranging subsystem error model can be solved by the following formula: in, represents the error covariance of the UWB ranging subsystem error model, is the distance information output by the sensor, True distance measurement between machines

[0063] 2. Model for optimizing subsystem error classification The above six subsystem error models face complicated situations in the process of spatiotemporal transmission. They need to be optimized according to different environmental conditions and task requirements, and only the subsystems with smaller collaborative errors are fused and spatiotemporally transmitted. According to the above subsystem error models, they are fused through the following grading indicators to obtain a relatively better subsystem error optimization model under the current environment and requirements.

[0064] Specifically, the specific implementation process of step S3 is as follows:

[0065] S3.1 sets the Class I indicator to the error covariance. The error covariance is solved by the sensor subsystem error model part, that is, For details, please see the error model of six sensor subsystems.

[0066] S3.2 Setting the error covariance threshold value: Determine the error covariance threshold value based on the complexity of the specific environment and specific needs, and set it based on experience.

[0067] S3.3 Preliminary screening: Calculate the error covariance of the above six types of subsystems respectively. PNT sources with error covariance greater than the error threshold value will not be considered; PNT source subsystem models with error covariance not greater than the error threshold value will enter the next optimization process.

[0068] The specific implementation process of step S4 is as follows: S4.1 Setting Type II indicators: Error covariance, error controllability , error influencing factors, error propagation mechanism and positioning continuity Indicators are set in five aspects and evaluated through radar charts. Figure 5 shown.

[0069] S4.2 Setting the threshold value of Class II indicators: Determine the threshold value of Class II indicators based on the complexity of the specific environment and specific needs; Among them, in S4.2, set the type II indicators (including error covariance, error controllability , error influencing factors, error propagation mechanism and positioning continuity ) The specific method of threshold value is as follows: S4.2.1 For the error covariance, the error model part of the sensor subsystem is used to solve it.

[0070] S4.2.2 Error controllability , the error controllability is a whole composed of the controllability alarm threshold, controllability risk and alarm time, and its expression is as follows:

[0071] in, is a function of the controllability, is the controllability alarm threshold, For controllable risks, is the controllability alarm time, The weights of controllability alarm threshold, integrity risk and alarm time.

[0072] S4.2.3 For error influencing factors, the error propagation mechanisms of different subsystems are different, including: 1. The inertial navigation subsystem is mainly affected by sensor noise, initial alignment error, sensor bias, temperature effect, mechanical vibration, geophysical model error, dynamic calibration error, environmental interference, cumulative error, etc. The score is between 50-60 points. The specific score is determined based on experience. During this score period, choose based on experience and proceed to step 6 according to the specific score, drawing a radar chart, as shown in the figure; 2. The satellite navigation subsystem is mainly affected by sensor noise, measurement error, receiver error, relativistic effect, earth rotation, environmental impact, etc. The score is between 60 and 70 points, and the specific score is determined based on experience; 3. The visual odometry scoring system is mainly affected by the accuracy of feature extraction and matching, image texture, lighting conditions, camera resolution, cumulative error, initialization error, scene depth change, motion blur, etc. The score is between 45-55 points, and the specific score is determined based on experience; 4. The laser radar odometer scoring system is mainly affected by sensor noise, measurement error, target reflection characteristics, multipath effect, time synchronization error, accumulation error, initialization error, point cloud matching error, etc. The score is between 65-75 points, and the specific score is determined based on experience; 5. The UWB subsystem is mainly affected by multipath effects, non-line-of-sight propagation, clock synchronization accuracy, signal attenuation, environmental factors, dynamic environment, etc. The score is between 55-65 points, and the specific score is determined based on experience; 6. The magnetometer scoring system is mainly affected by soft and hard iron interference, geomagnetic field changes, sensor zero bias, temperature drift, calibration error, external magnetic field interference, mechanical error, dynamic response, etc. The score is between 35-45 points, and the specific score is determined based on experience.

[0073] S4.2.4 Regarding the error propagation mechanism, the error propagation mechanism of different subsystems is different: for satellite navigation, mobile communications, UWB, etc., the error will not accumulate over time, so the score should be between 40-100; while the errors of subsystems such as inertial navigation, vision, magnetometer, etc. will accumulate over time, and the score should be appropriately reduced to between 20-40; the specific value can be set based on experience.

[0074] S4.2.5 For positioning continuity , positioning continuity The expression is as follows:

[0075] in, The function for locating continuity for this subsystem, is the spatial signal continuity of the subsystem, For the system continuity of this subsystem, are the relative weights of spatial signal continuity and system continuity for positioning continuity, and .

[0076] S4.3 Secondary screening: Draw the performance radar charts of the above six types of subsystem error models according to the five aspects of Class II indicators, calculate the area within each performance radar chart, and sort them from large to small according to the area, and preliminarily select the top four PNT sources as the final preferred results.

[0077] In step S5, the motion trajectory of the unmanned platform (such as an unmanned vehicle, an unmanned ship, or a drone) and its corresponding parameters are simulated by a trajectory generator.

[0078] 3. Error simulation model of multi-source heterogeneous PNT information sources Based on the above optimization results, the optimized subsystem error model parameters are obtained, and the above subsystem error models are fused through Kalman filtering to obtain the fused collaborative positioning error model. The specific steps are as follows: Aiming at the multi-source PNT information fusion of unmanned cluster under simulation denial conditions, the present invention discretizes the continuous time-domain Kalman filter. The system state equation and measurement equation after discretization are: In the formula

[0079] In the formula, It is an iteration cycle. represents the n-dimensional system state vector at time k, represents the m-dimensional measurement vector at time k; represents the one-step transfer matrix of the system from time k-1 to time k, represents the system noise at time k-1, is the system noise matrix, Represents the system measurement matrix. represents the system measurement noise, the subscript represents the time series, F represents the state equation, and G represents the random noise variable

[0080] The system noise and measurement noise in the state equation and measurement equation should have the following properties:

[0081] In the formula, Represents the expectation, discrete system error covariance matrix and measurement error covariance matrix , As well as the continuous measurement error covariance matrix and the systematic error covariance matrix , The relationship can be approximately expressed as (2.34) Therefore, for the unmanned cluster multi-source PNT information fusion under simulation denial conditions, the recursive equation of the discrete Kalman filter of the present invention can be modeled as: (2.35)

[0082] Among them, the first two items are the state prediction equation and the covariance prediction equation, the middle one is the filter gain equation, and the last two items are the updated state estimation equation and the covariance estimation equation. and Represent the variance matrices of system noise and measurement noise respectively, so as to solve the final fusion error covariance based on the above formula , realizing simulation analysis of multi-source heterogeneous navigation.

[0083] The technical advantages of the present invention are as follows: 1. Support error model analysis for multiple navigation methods, which almost covers several current mainstream navigation methods. When in use, the model will not be replaced again due to changes in navigation methods, thereby reducing development costs and improving compatibility.

[0084] 2. It has strong environmental adaptability and can perform error model analysis on non-fixed environmental parameters. Environmental changes will not affect the accuracy. This method can flexibly judge and assign error test models according to different input navigation methods to improve the model's adaptability.

[0085] 3. High flexibility. The error fusion model can be changed according to actual needs. The error fusion model can be flexibly selected and configured for navigation error analysis according to actual needs. According to the environmental information and sensor conditions at the time, the optimal combination is selected to minimize the error.

[0086] Any process or method description in the flowchart of the present invention or described in other ways herein can be understood as a module, segment or part of a code including one or more executable instructions for implementing the steps of a specific logical function or process, which can be implemented in any computer-readable medium for use by an instruction execution system, device or equipment, and the computer-readable medium can be any medium containing storage, communication, propagation or transmission programs for use by execution systems, devices or equipment, including read-only memories, magnetic disks or optical disks, etc.

[0087] In the description of this specification, the description with reference to the terms "embodiment", "example", etc. means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic representations of the above terms do not necessarily refer to the same embodiment or example. In addition, those skilled in the art can combine or combine different embodiments or examples described in this specification and the features therein without causing any contradiction.

[0088] Although the above content has shown and described the embodiments of the present invention, it can be understood that the above embodiments are exemplary and cannot be understood as limitations of the present invention. Ordinary technicians in the field can change, modify, replace, modify and other update operations on the above embodiments within the scope of the present invention.

Claims

1. A multi-source heterogeneous navigation source error model optimization method is applied to unmanned equipment navigation system, characterized in that: The multi-source heterogeneous navigation source includes: inertial information output by an inertial navigation subsystem, latitude and longitude altitude information output by a satellite navigation subsystem, a visual odometer scoring system, a laser radar odometer scoring system, relative measurement navigation information output by a UWB subsystem, and heading angle information output by a magnetometer scoring system; the method includes the following steps: S1. Calculate the error model of each sensor subsystem; S2. Initial calibration of each sensor; S3. Preliminary screening of sensor subsystems; calculate the error covariance of the above six subsystems respectively, and PNT sources with error covariance greater than the error threshold value are not considered; only other subsystem models that meet the error threshold value requirements can enter the next optimization process; S4. Secondary screening of sensors: Draw performance radar charts for the other subsystem models that meet the threshold requirements according to the five aspects of Class II indicators, calculate the area within each performance radar chart, and sort them from large to small according to the area, and select the top three PNT sources as the final optimization result. If there are less than three, keep the top two; S5. Simulate motion parameters: simulate the motion trajectory of the unmanned platform and its corresponding parameters through the trajectory generator; S6. Determine whether the number of sensors after the optimization is greater than or equal to 2; if so, perform simulation analysis, Kalman filter correction, and save simulation data; if not, directly save the simulation data; S7. Determine whether the simulation is finished, if so, proceed to step S8, if not, return to step S3; S8. Draw a curve of the fusion error changing over time.

2. The multi-source heterogeneous navigation source error model optimization method according to claim 1 is characterized in that: In step S1, the sensor subsystem error model includes a satellite navigation subsystem error model, an inertial navigation subsystem error model, a lidar subsystem error model, a visual navigation subsystem error model, a magnetometer subsystem error model and a UWB ranging subsystem error model.

3. The multi-source heterogeneous navigation source error model optimization method according to claim 2 is characterized in that: In the satellite navigation subsystem error model, the error model is: in, The factors that cause the navigation message error, satellite clock error, ionospheric delay, and tropospheric delay are respectively Satellite pseudo-ranging error, represents the measurement noise, It represents the error caused by multipath effect to pseudo-range measurement, represents the satellite navigation system error, represents the pseudorange, represents the receiver clock error, Represents the error caused by multipath.

4. The multi-source heterogeneous navigation source error model optimization method according to claim 2 is characterized in that: In the inertial navigation subsystem error model, the output relationship of the gyro component of the inertial navigation is expressed as: Among them, the actual angular velocity output of the gyro component The scale factor error is included in , coupling force, is the angular velocity, is the sensitivity and sensitivity coefficient Product, zero drift and random colored noise ; There is colored noise Modeled as a random constant and a first-order Markov process, as follows: The output relationship of the accelerometer component of inertial navigation is expressed as: Actual acceleration output of the acceleration component The same model is , is a constant drift, is random colored noise.

5. The multi-source heterogeneous navigation source error model optimization method according to claim 2, characterized in that: In the laser radar subsystem error model, the laser radar uses the time difference method to obtain distance information. The transmitting part transmits a laser beam to the target object. When the laser beam reaches the surface of the target object, it is reflected and received by the receiving part. The relative distance between the target point and the laser radar is measured by the time difference between the two pulses transmitted and received. The specific formula is as follows: c represents the speed of light, t represents time, and the scanning point position information is in spherical coordinates (r, , ) to record, according to the scanning point distance r, pitch angle and azimuth Convert it to the rectangular coordinate system of the laser radar carrier itself.

6. The multi-source heterogeneous navigation source error model optimization method according to claim 2, characterized in that: In the visual navigation subsystem error model, the relative position of visual measurement is modeled as: is the relative position output by the visual simulator in two adjacent frames. is the relative position truth value, is Gaussian white noise.

7. The multi-source heterogeneous navigation source error model optimization method according to claim 2, characterized in that: In the magnetometer subsystem error model, the three-axis components of the vector H measured by the magnetometer in the geographic coordinate system are: and ;in The combined force It always refers to the north direction, and D is the magnetic heading, which is expressed as: Since the value range of the magnetic heading angle is , and the range of the experimentally calculated inverse tangent function is ,and , in order to eliminate singular values, the calculation of magnetic heading is constrained as follows: The magnetometer is affected by its own errors and external magnetic field interference factors, and its measurement model is established as follows: is the sensitivity error matrix, is the non-orthogonal error, is the soft magnetic interference matrix, is the hard magnetic interference vector, is the magnetometer bias, is the Gaussian noise of the sensor, The true magnetic field vector value, Indicates the measured magnetic field vector value.

8. The multi-source heterogeneous navigation source error model optimization method according to claim 2, characterized in that: In the UWB ranging subsystem error model, two adjacent unmanned vehicles in the cluster assist in positioning by receiving the collaborative information of adjacent unmanned vehicles within the ranging range. Under the condition of good line of sight, the inter-machine ranging value based on UWB is modeled as: is the real distance measurement between machines. is the distance information output by the sensor, Indicates noise; if the current position is , the position of the i-th adjacent node is , then the UWB output is: The position measurement value is , the position measurement value of the adjacent node of the i-th vehicle is , Taylor expansion of the above formula is: In the formula is the position of the adjacent unmanned vehicle, Indicates noise; Error That is, modeled as: Further simplification yields: UWB error between nodes middle The part is the equivalent ranging noise that combines the position error of node i and the UWB ranging noise, and the noise variance is: In the formula is the position covariance of the ith node.

9. The multi-source heterogeneous navigation source error model optimization method according to claim 2, characterized in that: The specific implementation of step S3 is as follows: S3.1 calculation sets the Class I indicator as error covariance; the error covariance is solved by the sensor subsystem error model part; S3.2 Setting the error covariance threshold value: Determine the error covariance threshold value according to the complexity of the specific environment and specific requirements; S3.3 Preliminary screening: Calculate the error covariance of the above six subsystems respectively. PNT sources with error covariance greater than the error threshold value will not be considered; PNT source subsystem models with error covariance less than the error threshold value will enter the next optimization process; The specific implementation of step S4 is as follows: S4.1 Setting Type II indicators: Error covariance, error controllability , error influencing factors, error propagation mechanism and positioning continuity Indicators are set in five aspects and evaluated through radar charts; S4.2 Setting the threshold value of Class II indicators: Determine the threshold value of Class II indicators based on the complexity of the specific environment and specific needs; S4.3 Secondary screening: Draw the performance radar charts of the other subsystem models that meet the threshold requirements according to the five aspects of Class II indicators, calculate the area within each performance radar chart, and sort them from large to small according to the area, and select the top three PNT sources as the final preferred results. If there are less than three, retain the top two.

10. Multi-source heterogeneous navigation source error model simulation system, characterized by: The simulation system is used to implement the multi-source heterogeneous navigation source error model optimization method according to any one of claims 1-9, and the system includes six types of sensor subsystem error models, error fusion transmission models, selection models and fusion simulation models.

Images