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

Abstract

The application discloses a multi-source heterogeneous navigation source error model optimization method and a simulation system, which is applied to an unmanned equipment navigation system and comprises the following steps: S1, calculating error models of each sensor subsystem; S2, initial calibration; S3, primary screening of the sensor subsystem; S4, secondary screening of the sensor; S5, simulating motion parameters; S6, judging whether the optimized sensor is greater than or equal to two; if yes, performing simulation analysis, Kalman filter correction and saving simulation data; if no, directly saving the data; S7, judging whether the simulation is finished, if yes, entering step S8, and if no, returning to step S3; and S8, drawing a curve. The simulation model can be compatible with multiple information sources simultaneously, corresponding navigation models are constructed for different information sources, the model can be selected flexibly according to the information sources, different matrix types can be selected flexibly according to actual use environments, the compatibility is stronger, the coverage is wider, the flexibility is higher, and the environmental adaptability is better.

Description

Technical Field

[0001] This invention relates to unmanned technology and navigation technology, and in particular to a method and simulation system for optimizing error models of multi-source heterogeneous navigation sources. Background Technology

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

[0003] In recent years, the rapid rise of unmanned technology and the widespread adoption of location services have placed higher demands on reliable navigation systems. Global Navigation Satellite System (GNSS) is a common outdoor positioning method. However, GNSS has extremely high environmental requirements. In complex environments, GNSS satellite signals are blocked by obstructions, resulting in weakened signal strength, increased noise, decreased accuracy, and data loss. This makes it impossible for GNSS to provide accurate positioning, and the availability, continuity, and reliability of GNSS PNT service cannot be guaranteed.

[0004] Because satellite navigation systems are easily interfered with and cannot be used indoors, current navigation technologies primarily employ integrated navigation or micro PNT (Positioning and Navigation Technology) methods for positioning and navigation. However, even with these methods, navigation inevitably suffers from errors due to various reasons. Current error analysis models for different navigation methods are mostly incomplete, supporting only error analysis for 2-3 methods and lacking effective compatibility with other navigation tools. Furthermore, navigation error models are relatively fixed and cannot be adapted to environmental factors, resulting in insufficient flexibility. Current PNT error analysis models are also inflexible and limited, mostly applicable only to 1-3 navigation methods, such as integrated navigation systems combining GPS and inertial navigation systems, sensor fusion-based integrated navigation systems, GPS integrated navigation systems, and Doppler integrated navigation systems. These systems operate on fixed patterns and cannot adapt their coordination methods to environmental changes.

[0005] The existing problems mainly focus on the following three points:

[0006] 1. The model is singular, has a limited scope of application, and poor compatibility. Once the navigation method is changed, the error model needs to be rebuilt, which is time-consuming, labor-intensive, and increases development costs.

[0007] 2. Poor environmental adaptability; it can only perform error model analysis for fixed environmental parameters. Once the environment changes, it will seriously affect the accuracy, resulting in unstable and inaccurate error model results, and the results will not have reference value.

[0008] 3. Low flexibility; the error fusion model cannot be modified according to actual needs. Summary of the Invention

[0009] To address the problems existing in the prior art, the present invention aims to provide a method and simulation system for optimizing the error model of multi-source heterogeneous navigation sources. This simulation model can be compatible with multiple information sources simultaneously, 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 different actual usage environments. It has stronger compatibility, wider coverage, higher flexibility, better environmental adaptability, and powerful functions.

[0010] To achieve the above objectives, this invention provides a method for optimizing the error model of multi-source heterogeneous navigation sources, applied to unmanned equipment navigation systems. The multi-source heterogeneous navigation sources include: inertial information output by an inertial navigation subsystem, latitude and longitude altitude information output by a satellite navigation subsystem, visual odometry, lidar odometry, relative measurement navigation information output by a UWB subsystem, and heading angle information output by a magnetometer subsystem. The method includes the following steps:

[0011] S1. Calculate the error model for each sensor subsystem;

[0012] S2. Initial calibration of each sensor;

[0013] S3. Initial screening of sensor subsystems; calculate the error covariance of the above six types of subsystems respectively. PNT sources with error covariance greater than the error threshold are not considered. Only other subsystem models that meet the error threshold requirements can enter the next optimization process.

[0014] S4. Secondary sensor screening: For the other subsystem models that meet the threshold requirements, draw their respective performance radar charts according to the five aspects of Category II indicators, calculate the area within each performance radar chart, and sort them from largest to smallest area. Select the top three PNT sources as the final preferred results. If there are fewer than three, retain the first two.

[0015] S5. Simulate motion parameters; Simulate the motion trajectory of the unmanned platform and its corresponding parameters using a trajectory generator;

[0016] S6. Determine if the optimized sensor count is greater than or equal to 2; if yes, perform simulation analysis, Kalman filter correction, and save the simulation data; if no, directly save the simulation data.

[0017] S7. Determine whether the simulation has ended. If yes, proceed to step S8; otherwise, return to step S3.

[0018] S8. Plot the curve of fusion error over time.

[0019] Furthermore, in step S1, the sensor subsystem error model includes the satellite navigation subsystem error model, the inertial navigation subsystem error model, the lidar subsystem error model, the visual navigation subsystem error model, the magnetometer subsystem error model, and the UWB ranging subsystem error model.

[0020] Furthermore, in the error model of the satellite navigation subsystem, the error is modeled as follows:

[0021]

[0022] in, These represent the effects of factors such as navigation message error, satellite clock error, ionospheric delay, and tropospheric delay on the first... Satellite pseudo ranging error, Represents measurement noise. This indicates the error introduced by the multipath effect into pseudorange measurement. Indicates the error of the satellite navigation system. Indicates pseudorange, Indicates receiver clock bias. This indicates the error caused by multiple paths.

[0023] Furthermore, in the error model of the inertial navigation subsystem, the output relationship of the gyroscope component of the inertial navigation system is expressed as follows:

[0024]

[0025] Among them, the actual angular velocity output of the gyroscope component Includes scaling factor error Coupling force Angular velocity, Sensitivity and sensitivity coefficient Product, Zero-point drift and random colored noise ;including colored noise Modeled as random constants And a first-order Markov process, specifically as follows:

[0026]

[0027] The output relationship of the accelerometer component in inertial navigation can generally be expressed as:

[0028]

[0029] Actual acceleration output of the acceleration component The same model is used in China. , For constant drift, It is random colored noise.

[0030] Furthermore, in the error model of the lidar subsystem, the lidar uses the time difference method to obtain distance information. The transmitting part emits a laser beam towards the target object. When the laser beam reaches the surface of the target object, it is reflected and received by the receiving part. By using the time difference between the emitted and received pulses, the relative distance between the target point and the lidar is measured. The specific formula is as follows:

[0031]

[0032] c represents the speed of light, t represents time, and the scan point position information is expressed in spherical coordinates (r, ...). , Record the data based on the distance r from the scan point and the pitch angle. and azimuth It can be converted to the Cartesian coordinate system of the lidar carrier itself.

[0033] Furthermore, in the visual navigation subsystem error model, the relative position measured visually is modeled as follows:

[0034]

[0035] This represents the relative position output by the visual simulator in two adjacent frames. For the relative position truth value, It is Gaussian white noise.

[0036] 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 as follows: , and ;in , The combined force Always pointing north, D is the magnetic heading, which can be represented as:

[0037]

[0038] Since the range of magnetic heading angle is The range of the arctangent function calculated experimentally is ,and To eliminate singular values, the calculation of magnetic heading was constrained, as follows:

[0039]

[0040] Magnetometers are affected by their own errors and external magnetic field interference. Their measurement model can be established as follows:

[0041]

[0042] The sensitivity error matrix is... Non-orthogonal error, For soft magnetic interference matrix, This is the hard magnetic interference vector. To achieve zero bias in the magnetometer, The Gaussian noise of the sensor, The actual magnetic field vector value, This represents the measured magnetic field vector value.

[0043] Furthermore, in the error model of the UWB ranging subsystem, two adjacent unmanned vehicles in the cluster can receive cooperative information from adjacent unmanned vehicles within the ranging range to assist in localization. Under good line-of-sight conditions, the inter-vehicle ranging value based on UWB can be modeled as follows:

[0044]

[0045] For actual distance measurement between machines, The ranging 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:

[0046]

[0047] Among them, the position measurement value is The position measurement value of the i-th vehicle's adjacent node is Expanding the above equation using Taylor series, we get:

[0048]

[0049] In the formula The position of adjacent nodes, Indicates noise; error It can be modeled as follows:

[0050]

[0051] Further simplification yields:

[0052]

[0053] UWB error between nodes middle Part of it is the equivalent ranging noise that combines the position error of node i and the UWB ranging noise, with the noise variance being:

[0054]

[0055] In the formula Let be the position covariance of the i-th node.

[0056] Furthermore, the specific implementation method of step S3 is as follows:

[0057] S3.1 The calculation of the Class I index is set as the error covariance; the error covariance is solved by the error model of the sensor subsystem;

[0058] S3.2 Set the error covariance threshold: Determine the error covariance threshold based on the specific environmental complexity and requirements; different threshold values ​​can be set in the radar chart according to the actual situation.

[0059] S3.3 Initial Screening: Calculate the error covariance of the above six types of subsystems respectively. PNT sources with an error covariance greater than the error threshold are not considered. PNT source subsystem models with an error covariance not greater than the error threshold proceed to the next optimization process.

[0060] The specific implementation method of step S4 is as follows:

[0061] S4.1 Set up Category II indicators: Include error covariance and error controllability. Factors affecting error, error propagation mechanism, and positioning continuity Five indicators are set, and the evaluation is carried out using a radar chart.

[0062] S4.2 Set threshold values ​​for Category II indicators: Determine the threshold values ​​for Category II indicators based on the specific environmental complexity and specific requirements;

[0063] S4.3 Secondary Screening: For the other subsystem models that meet the threshold requirements, draw their respective performance radar charts according to the five aspects of the Class II indicators, calculate the area within each performance radar chart, and sort them from largest to smallest area. Select the top three PNT sources as the final preferred results. If there are fewer than three, retain the first two.

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

[0065] Beneficial effects:

[0066] The error simulation system of this invention supports six PNT information sources, including lidar, satellite navigation, mobile communication, inertial navigation, UWB, vision, magnetometer, and gravity field. Compared with traditional navigation error simulation models, the error simulation model of this invention is richer, more convenient to use, and simpler to operate. Furthermore, this invention designs a "matrix selection" process, which integrates these subsystems and performs optimal combination based on the environmental information and sensor conditions at the time, selecting the optimal combination to minimize error.

[0067] This method supports error analysis for up to six navigation modes, covering almost all current mainstream navigation modes. It does not require model changes due to different navigation modes, resulting in better compatibility and greater applicability.

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

[0069] This method allows for flexible selection of the configuration error fusion model for navigation error analysis based on actual needs. Attached Figure Description

[0070] Figure 1 The present invention illustrates the method for optimizing the error model of multi-source heterogeneous navigation sources and the structure diagram of the simulation system.

[0071] Figure 2 A schematic diagram of a lidar ranging system according to the present invention is shown;

[0072] Figure 3 A schematic diagram of the Earth's magnetic field lines distribution according to the present invention is shown;

[0073] Figure 4 A schematic diagram illustrating the measurement principle of the triaxial magnetometer according to the present invention is shown;

[0074] Figure 5 A radar diagram illustrating the PNT service performance index of the Class II index according to the present invention is shown. Detailed Implementation

[0075] The technical solution of the present invention will now be clearly and completely described with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0076] In the description of this invention, it should be noted that the terms "center," "upper," "lower," "left," "right," "vertical," "horizontal," "inner," and "outer," etc., indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings. They are used only for the convenience of describing the invention and for simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, they should not be construed as limitations on the invention. Furthermore, the terms "first," "second," and "third" are used for descriptive purposes only and should not be construed as indicating or implying relative importance.

[0077] In the description of this invention, it should be noted that, unless otherwise explicitly specified and limited, the terms "installation," "connection," and "linking" should be interpreted broadly. For example, they can refer to a fixed connection, a detachable connection, or an integral connection; they can refer to a mechanical connection or an electrical connection; they can refer to a direct connection or an indirect connection through an intermediate medium; and they can refer to the internal connection of two components. Those skilled in the art can understand the specific meaning of the above terms in this invention according to the specific circumstances.

[0078] The following combination Figures 1-5 Specific embodiments of the present invention will be described in detail below. It should be understood that the specific embodiments described herein are for illustrative and explanatory purposes only and are not intended to limit the present invention.

[0079] The multi-source heterogeneous navigation source error model optimization method and simulation system of the present invention support multiple PNT information sources, including: lidar, satellite navigation, mobile communication, inertial navigation, UWB, vision, magnetometer, gravity field, etc. That is, the multi-source heterogeneous PNT sources that the system can simulate include: inertial information output by the inertial navigation subsystem, latitude and longitude altitude information output by the satellite navigation subsystem, relative measurement navigation information output by the visual odometry subsystem, lidar odometry subsystem, and UWB subsystem, and heading angle information output by the magnetometer subsystem, totaling six information sources. Based on the above information sources, by constructing an error fusion propagation model, a selection model, and a fusion simulation model, an error simulation system and method for multi-source heterogeneous PNT information sources are realized.

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

[0081] Among them, the error models of the six types of sensor subsystems are error models constructed based on the error characteristics of various types of sensors. They are mainly used for calculating the error covariance index in the initial screening and secondary screening.

[0082] The subsystem error classification and optimization model is used to screen sensor combinations that can be used for fusion calculation in this environment.

[0083] The error simulation model for multi-source heterogeneous PNT information sources is used to combine selected sensors and perform fusion calculations. It is used to solve for the fusion positioning error, hence the name "error simulation model for multi-source heterogeneous PNT information sources."

[0084] like Figure 1 As shown, the preferred method for the multi-source heterogeneous navigation source error model according to the present invention includes the following steps:

[0085] S1. Setup: Calculate the error model of each sensor subsystem; calculate the positioning error covariance of each sensor.

[0086] S2. Initial calibration of each sensor.

[0087] S3. Initial screening of sensor subsystems; calculate the error covariance of the above six types of subsystems respectively. PNT sources with error covariance greater than the error threshold are not considered; other subsystem models can then proceed to the next optimization process.

[0088] S4. Secondary sensor screening: Draw the performance radar charts of the above six types of subsystem error models according to the five aspects of the Class II index, calculate the area within each performance radar chart, and sort them in descending order of area to initially screen out the top four PNT sources as the final preferred results.

[0089] S5. Simulate motion parameters; Using a trajectory generator, simulate the motion trajectory and corresponding parameters of unmanned platforms (such as unmanned vehicles, unmanned ships, and drones). Simulating motion parameters is essentially a simulation method.

[0090] S6. Determine if the number of optimized sensors is greater than or equal to 2; if yes, perform simulation analysis, Kalman filter correction, and save the simulation data; if not, directly save the simulation data. Based on the above optimization results, obtain the optimized subsystem error model parameters, and fuse the above subsystem error models through Kalman filtering to obtain the fused cooperative positioning error model. Generally, 3-4 optimal sensors will be retained, but if the accuracy is too poor during the initial screening, less than 2 may remain. If less than 2 remain, fusion calculation cannot be performed. Then there is no need to perform fusion simulation. Therefore, it is set to greater than or equal to 2 here.

[0091] S7. Determine if the simulation has ended. If yes, proceed to step S8; otherwise, return to step S3.

[0092] S8. Plot the curve of fusion error over time.

[0093] Specifically, the sensor subsystem error model includes the satellite navigation subsystem error model, the inertial navigation subsystem error model, the lidar subsystem error model, the visual navigation subsystem error model, the magnetometer subsystem error model, and the UWB ranging subsystem error model, as detailed below:

[0094] (1) Error model of satellite navigation subsystem

[0095] In satellite navigation systems, the errors collected by receivers are mainly affected by factors such as atmospheric refraction, building obstruction, and multipath effects.

[0096] Taking into account external environmental factors such as the ionosphere, troposphere, multipath effects, and geomagnetic interference, as well as the influence of satellite clock errors and satellite ephemeris errors, the commonly used error modeling for satellite navigation systems is as follows:

[0097]

[0098] These represent the effects of factors such as navigation message error, satellite clock error, ionospheric delay, and tropospheric delay on the first... Satellite pseudo ranging error, Representing measurement noise, in conventional satellite navigation receivers, the above factors will cause approximately 10 meters of noise. Positioning error. This indicates the error introduced by the multipath effect into pseudorange measurement. Indicates the error of the satellite navigation system. Indicates pseudorange, Indicates receiver clock bias. The error caused by multipath propagation is modeled in three states for satellite output: normal state, interfered state, and rejected state. The normal state is modeled using Gaussian white noise; in the interfered state, the standard deviation coefficient of the model is increased; and in the rejected state, the output is directly zero. Therefore, the error covariance of the satellite navigation subsystem error model can be solved using the following formula:

[0099]

[0100] (2) Error model of inertial navigation subsystem

[0101] Inertial navigation systems (INS) do not rely on any external information or radiate energy, offering excellent stealth and immunity to electromagnetic interference. They can operate globally, in all weather conditions, and at all times, in the air or on the Earth's surface. Strapdown inertial navigation systems (SINS) are simple in structure, small in size, lightweight, low in cost, easy to maintain, and highly reliable. Furthermore, redundancy techniques can enhance their fault tolerance, making them suitable for unmanned vehicle swarms. Therefore, in this project, both high-precision and low-precision navigation INS in the swarm utilize SINS as the core for information fusion navigation.

[0102] The output relationship of the gyroscope component in inertial navigation can generally be expressed as:

[0103]

[0104] Actual angular velocity output of gyroscope component Includes scaling factor error Coupling force Angular velocity, Sensitivity and sensitivity coefficient Product, Zero-point drift and random colored noise Among them, colored noise Modeled as random constants and first-order Markov processes ,Right now:

[0105]

[0106] The output relationship of the accelerometer component in inertial navigation can generally be expressed as:

[0107]

[0108] Actual acceleration output of the acceleration component The same model is used in China. , For constant drift, The noise is random colored noise. Apart from colored noise, other noise can be largely eliminated using a turntable; therefore, this project targets colored noise. The position output error, modeled as a first-order Markov process, can be modeled as follows:

[0109]

[0110] Among them, L, and h represent longitude, latitude, and altitude, respectively. , and Let each represent its derivative, where , , Indicates the northward velocity, eastward velocity, and normal velocity. Indicates zero-point drift error. It is random white noise.

[0111] This invention requires consideration of altitude information during simulation design. However, the altitude channel of an inertial navigation system is unstable, meaning it is easily affected by errors. The theoretical error modeling based on the altitude channel of inertial navigation is as follows:

[0112]

[0113]

[0114] , e represents the natural logarithm, when When the value is greater than 1, the altitude channel error will have an impact with respect to the square of time, generally showing a trend of emission. Therefore, in the simulation of this project, when the unmanned vehicle is operating for long durations, the altitude channel of the pure inertial navigation system cannot be used alone for extended periods; other navigation information sources need to be introduced to correct the node altitude information. The error covariance of the inertial navigation subsystem error model can then be solved using the following formula:

[0115]

[0116] in, This represents the error covariance of the inertial navigation subsystem error model. Expressing expectations, These represent longitude, latitude, and altitude, respectively.

[0117] (3) Error model of lidar subsystem

[0118] LiDAR (Light Detection and Ranging) uses the time difference method to obtain distance information. The transmitting unit emits a laser beam towards the target object. This laser beam is reflected upon reaching the target surface and received by the receiving unit. The relative distance between the target point and the LiDAR is measured by the time difference between the emitted and received pulses. In other words:

[0119]

[0120] Where c represents the speed of light, and t represents the position information of the time scan point in spherical coordinates (r, , Record it, such as Figure 2 As shown, based on the distance r from the scanning point and the pitch angle... and azimuth It can be converted to the Cartesian coordinate system of the lidar carrier itself.

[0121] Distance error Pitch angle error is And azimuth error is Based on actual test data, the error in the ranging information of the lidar only exhibits Gaussian white noise during simulation, with a mean of 0 and a variance of 0.01m. The error output of the lidar odometer, however, utilizes a Gaussian Markov mathematical model.

[0122] Therefore, the error covariance of the lidar subsystem error model can be solved by the following formula:

[0123]

[0124] (4) Error model of visual navigation subsystem

[0125] Visual navigation methods utilize cameras to acquire information about the surrounding environment and then calculate the camera's own motion from this observed information. A commonly used approach based on nodes is the visual perception module principle.

[0126] Points in camera coordinate system Its corresponding image coordinates are According to the similarity relation of triangles, the following holds:

[0127]

[0128] Where x and y represent the image The coordinates are given by f, where f represents the focal length.

[0129] Assuming the image is in pixel coordinate system Scaled on axis times, in Scaled on axis The origin has shifted by a factor of 100. ,but The coordinates are converted to pixel coordinates as follows:

[0130]

[0131] definition , We can obtain:

[0132]

[0133] In the formula, the focal length is in meters. , The unit is pixels per meter, therefore , , , The unit is pixels. The matrix form of the above formula is:

[0134]

[0135] After obtaining the above formula, it needs to be normalized to obtain the target point. The projection onto the camera's normalized plane yields the point. The normalized coordinates. These coordinates are equivalent to the coordinates in front of the camera. A point on the plane at which the plane is defined becomes the normalized plane. From the above equation, we know that point... Relative to the optical center line of sight azimuth and the angle of view They are respectively:

[0136]

[0137] As we know from the principles of visual measurement, visual positioning calculates world coordinates from pixel coordinates. Therefore, the positioning error of a point in space is caused by the error in world coordinates due to the error in pixel coordinates. The world coordinate system can be transformed into the camera coordinate system through rotation and translation matrices, thereby obtaining the relative azimuth of the target with respect to the camera. However, the fitting error of the image feature points projected onto the target in the pixel coordinate system is random. Therefore, the errors in the calculated azimuth and pitch angles of that point are also random, and can generally be modeled as Gaussian white noise.

[0138] For the relative position measured visually, the present invention models it as follows:

[0139]

[0140] This represents the relative position output by the visual simulator in two adjacent frames. For the relative position truth value, The noise is Gaussian white noise. The visual odometry position calculation uses the SLAM algorithm, and the error generated during the calculation process is modeled as a first-order Markov algorithm, random numbers, and accumulated Gaussian white noise. Therefore, the error covariance of the visual navigation subsystem can be solved using the following formula:

[0141]

[0142] (5) Error model of magnetometer subsystem

[0143] The actual distribution of magnetic field lines generated by the Earth's magnetic field is as follows: Figure 3 As shown. Magnetic field lines inside the Earth point from the geomagnetic south pole to the geomagnetic north pole, while in outer space they point from the geomagnetic north pole to the geomagnetic south pole. The distribution of magnetic field lines near the Earth is characterized by their horizontal direction near the equator, where the magnetic field is weakest, and their almost perpendicular direction near the magnetic poles, where the magnetic field is strongest. The geomagnetic south and north poles are not located on the Earth's axis of rotation, and the geomagnetic poles do not coincide with the geographic north and south poles.

[0144] Magnetometers can measure the magnetic field vector at any location on Earth. This project uses them as a navigation information source for unmanned vehicles (UAVs) to calculate their heading angles. The method for calculating yaw angles using magnetometers involves measuring the direction and intensity of the Earth's magnetic field and then using this magnetic field data to calculate the UAV's yaw angle. For example... 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 as follows: , and .in , The combined force Always pointing north, D is the magnetic heading, which can be represented as:

[0145]

[0146] Since the range of magnetic heading angle is The range of the arctangent function calculated experimentally is ,and To eliminate singular values, this invention constrains the calculation of magnetic heading, namely:

[0147]

[0148] Magnetometers are affected by their own errors and external magnetic field interference. Their measurement model can be established as follows:

[0149]

[0150] The sensitivity error matrix is... Non-orthogonal error, For soft magnetic interference matrix, This is the hard magnetic interference vector. To achieve zero bias in the magnetometer, The Gaussian noise of the sensor, The actual magnetic field vector value, This represents the measured magnetic field vector value.

[0151] Besides the non-orthogonality of the sensor's three axes causing output error, sensitivity error and zero-point offset error also affect the sensor's output. Therefore, under the influence of triaxial non-orthogonality, sensitivity error, and zero-point offset error, the sensor's output can be expressed as:

[0152]

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

[0154]

[0155] The actual calculated values ​​of the sensor are obtained, including zero-point drift error, triaxial sensitivity error, and triaxial non-orthogonality error. The error covariance of the magnetometer subsystem error model can then be solved using the following formula:

[0156]

[0157] in, This represents the error covariance of the magnetometer subsystem error model. This represents the average value of the sensor output value measured multiple times.

[0158] (6) Error model of UWB ranging subsystem

[0159] UWB signals have advantages such as high data transmission rate, strong penetration, strong anti-interference ability, and high multipath resolution, and are often used for inter-vehicle ranging. This invention utilizes UWB ranging information to achieve cooperative navigation within an unmanned vehicle swarm. UWB ranging errors mainly arise from factors such as clock errors and antenna delays. Adjacent unmanned vehicles in the swarm can receive cooperative information from adjacent unmanned vehicles within the ranging range to assist in positioning. Under good line-of-sight conditions, the inter-vehicle ranging value based on UWB can be modeled as follows:

[0160]

[0161] For actual distance measurement between machines, The ranging 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:

[0162]

[0163] Among them, the position measurement value is The position measurement value of the adjacent node of the i-th vehicle is Expanding the above equation using Taylor series, we get:

[0164]

[0165] In the formula The location of adjacent driverless vehicles. Indicates noise. error It can be modeled as follows:

[0166]

[0167] Further simplification yields:

[0168]

[0169] UWB error between nodes middle Part of it is the equivalent ranging noise that combines the position error of node i and the UWB ranging noise, with the noise variance being:

[0170]

[0171] In the formula Let be 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:

[0172]

[0173] in, This represents the error covariance of the UWB ranging subsystem error model. The ranging information output by the sensor. For actual distance measurement between machines

[0174] (II) Subsystem Error Hierarchical Optimization Model

[0175] The aforementioned six subsystem error models face complex situations during spatiotemporal transmission. Therefore, it is necessary to optimize each subsystem error model based on different environmental conditions and task requirements, performing error fusion only on subsystems with smaller cooperative errors and then transmitting them spatiotemporally. Based on the above subsystem error models, they are fused using the following hierarchical indicators to obtain a relatively better subsystem error optimization model under the current environment and requirements.

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

[0177] S3.1 sets the type I index as error covariance. The error covariance is solved by the error model of the sensor subsystem, i.e., by... For details, please refer to the error models of the six types of sensor subsystems.

[0178] S3.2 Set the error covariance threshold: Determine the error covariance threshold based on the specific environmental complexity and requirements, and set it based on experience.

[0179] S3.3 Initial Screening: Calculate the error covariance of the above six types of subsystems respectively. PNT sources with error covariance greater than the error threshold are not considered. PNT source subsystem models with error covariance not greater than the error threshold enter the next optimization process.

[0180] The specific implementation process of step S4 is as follows:

[0181] S4.1 Set up Category II indicators: Include error covariance and error controllability. Factors affecting error, error propagation mechanism, and positioning continuity Five indicators are set, and the evaluation is carried out using a radar chart, as follows: Figure 5 As shown.

[0182] S4.2 Set threshold values ​​for Category II indicators: Determine the threshold values ​​for Category II indicators based on the specific environmental complexity and specific requirements;

[0183] In section S4.2, Class II indicators (including error covariance and error controllability) are defined. Factors affecting error, error propagation mechanism, and positioning continuity The specific method for setting thresholds is as follows:

[0184] S4.2.1 The error covariance is solved by the error model of the sensor subsystem.

[0185] S4.2.2 Regarding error controllability Error controllability is a whole consisting of controllability alarm threshold, controllable risk, and alarm time, and its expression is as follows:

[0186]

[0187] in, This is a function of controllability. As a controllability alarm threshold, To controllable risks, For controllable alarm time, Weights are assigned to controllability alarm threshold, integrity risk, and alarm time.

[0188] S4.2.3 Regarding the factors affecting error, the error propagation mechanism differs for different subsystems, specifically including:

[0189] 1. The inertial navigation subsystem is mainly affected by sensor noise, initial alignment error, sensor deviation, temperature effect, mechanical vibration, geophysical model error, dynamic calibration error, environmental interference, and cumulative error. The score is between 50 and 60 points. The specific score is determined based on experience. Within this score range, the selection is based on experience. The sixth step is to draw the radar chart based on the specific score, as shown in the figure.

[0190] 2. The satellite navigation subsystem is mainly affected by sensor noise, measurement error, receiver error, relativistic effects, Earth's rotation, and environmental factors, and scores between 60 and 70 points. The specific score is determined based on experience.

[0191] 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 changes, motion blur, etc. The score is between 45 and 55 points, and the specific score is determined based on experience.

[0192] 4. The lidar 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 and 75 points, and the specific score is determined based on experience.

[0193] 5. The UWB subsystem is mainly affected by multipath effects, non-line-of-sight propagation, clock synchronization accuracy, signal attenuation, environmental factors, and dynamic environment, and scores between 55 and 65 points. The specific score is determined based on experience.

[0194] 6. The magnetometer scoring system is mainly affected by soft and hard iron interference, changes in the geomagnetic field, sensor zero bias, temperature drift, calibration error, external magnetic field interference, mechanical error, dynamic response, etc. The score is between 35 and 45 points, and the specific score is determined based on experience.

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

[0196] S4.2.5 Regarding the continuity of positioning Positioning continuity The expression is as follows:

[0197]

[0198] in, The function for determining the continuity of this subsystem. For the spatial signal continuity of this subsystem, For the system continuity of this subsystem, These are the relative weights of spatial signal continuity and system continuity, respectively, for location continuity, and... .

[0199] S4.3 Secondary Screening: The above six types of subsystem error models are plotted according to the five aspects of the Class II index. The area within each performance radar chart is calculated and sorted from largest to smallest area. The top four PNT sources are initially selected as the final preferred results.

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

[0201] (III) Error Simulation Model of Multi-Source Heterogeneous PNT Information Source

[0202] Based on the above optimization results, the optimized subsystem error model parameters are obtained. Then, the subsystem error models are fused using Kalman filtering to obtain the fused cooperative positioning error model. The specific steps are as follows:

[0203] To address the fusion of multi-source PNT information in unmanned swarms under simulation rejection conditions, this invention discretizes the continuous-time-domain Kalman filter. The discretized system state equation and measurement equation are as follows:

[0204]

[0205] In the formula

[0206]

[0207] In the formula, The iteration period is defined as follows. This represents the state vector of the n-dimensional system at time k. Represents the m-dimensional measurement vector at time k; This represents the one-step transition matrix of the system from time k-1 to time k. This represents the system noise at time k-1. Here is the system noise matrix. This represents the system measurement matrix. This represents system measurement noise, with subscripts indicating time series, F representing the state equation, and G representing random noise variables.

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

[0209]

[0210] In the formula, Represents the expected value, the discrete system error covariance matrix, and the measurement error covariance matrix. , and the covariance matrix of continuous measurement error and the covariance matrix of systematic error , The relationship can be approximated as:

[0211] (2.34)

[0212] Therefore, for the fusion of multi-source PNT information in unmanned swarms under simulation rejection conditions, the recursive equation of the discrete Kalman filter in this invention can be modeled as follows:

[0213] (2.35)

[0214] The first two terms are the state prediction equation and the covariance prediction equation, the middle term is the filter gain equation, and the last two terms are the updated state estimation equation and the covariance estimation equation. and Let represent the variance matrices of the system noise and the measurement noise, respectively. Then, based on the above formula, the final fused error covariance can be calculated. This enables simulation analysis of multi-source heterogeneous navigation.

[0215] The technical advantages of this invention are as follows:

[0216] 1. Supports error model analysis for multiple navigation methods, covering almost all current mainstream navigation methods. When using it, the model will not need to be changed again due to changes in navigation methods, thereby reducing development costs and improving compatibility.

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

[0218] 3. High flexibility: The error fusion model can be modified according to actual needs. It allows for flexible selection and configuration of the error fusion model for navigation error analysis based on the environmental information and sensor conditions at the time, selecting the optimal combination to minimize errors.

[0219] Any process or method described in the flowcharts of this invention or otherwise herein can be understood as representing a module, segment, or portion of code comprising one or more executable instructions for implementing a particular logical function or process, achievable on any computer-readable medium for use by an instruction execution system, apparatus, or device. The computer-readable medium can be any medium containing a program for storage, communication, propagation, or transmission for use by an execution system, apparatus, or device, including read-only memory, magnetic disks, or optical disks.

[0220] In the description of this specification, references to terms such as "embodiment," "example," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of the present invention. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, those skilled in the art can combine or combine the different embodiments or examples described in this specification and the features therein without causing contradiction.

[0221] While embodiments of the present invention have been shown and described above, it is understood that the above embodiments are exemplary and should not be construed as limiting the present invention. Those skilled in the art can make changes, modifications, substitutions, and alterations to the above embodiments within the scope of the present invention.

Claims

1. A method for optimizing error models of multi-source heterogeneous navigation sources, applied to unmanned equipment navigation systems, characterized in that, The multi-source heterogeneous navigation sources include: inertial information output by the inertial navigation subsystem, latitude and longitude altitude information output by the satellite navigation subsystem, relative measurement navigation information output by the visual odometry subsystem, lidar odometry subsystem, UWB subsystem, and heading angle information output by the magnetometer subsystem; the method includes the following steps: S1. Calculate the error model for each sensor subsystem; S2. Initial calibration of each sensor; S3. Initial screening of sensor subsystems; set the error covariance as the Class I index, and calculate the error covariance of the six types of subsystems respectively. PNT sources with error covariance greater than the error threshold are not considered; only other subsystem models that meet the error threshold requirements can enter the next optimization process; S4. Secondary sensor screening: For the other subsystem models that meet the error threshold requirements, draw their respective performance radar charts according to the five aspects of Category II indicators, calculate the area within each performance radar chart, sort them from largest to smallest area, and select the top three PNT sources as the final preferred results. If there are fewer than three, retain the first two. S5. Simulate motion parameters; Simulate the motion trajectory of the unmanned platform and its corresponding parameters using a trajectory generator; S6. Determine whether the number of sensors after optimization is greater than or equal to 2; if yes, perform simulation analysis, Kalman filter correction, and save the simulation data; if no, directly save the simulation data. S7. Determine whether the simulation has ended. If yes, proceed to step S8; otherwise, return to step S3. S8. Plot the curve of fusion error over time; In step S1, the sensor subsystem error model includes the satellite navigation subsystem error model, the inertial navigation subsystem error model, the lidar subsystem error model, the visual navigation subsystem error model, the magnetometer subsystem error model, and the UWB ranging subsystem error model. The process of secondary screening of the six subsystem error models in step S4 is as follows: S4.1 Set up Category II indicators: Include error covariance and error controllability. Factors affecting error, error propagation mechanism, and positioning continuity Five aspects are set as Category II indicators, which are evaluated using radar charts; S4.2 Set threshold values ​​for Category II indicators: Determine the threshold values ​​for Category II indicators based on the specific environmental complexity and specific requirements; S4.3 Secondary screening: For the subsystem error models that meet the threshold requirements, draw their respective performance radar charts according to the five aspects of the Class II indicators, calculate the area within each performance radar chart, sort them from largest to smallest area, and select the top three PNT sources as the final preferred results. If there are fewer than three, retain the first two.

2. The method for optimizing the error model of multi-source heterogeneous navigation sources according to claim 1, characterized in that, In the error model of the satellite navigation subsystem, the error is modeled as follows: ； , , , The factors representing the effects of navigation message error, satellite clock error, ionospheric delay, and tropospheric delay on the first... Satellite pseudo ranging error, At the speed of light, Represents measurement noise. This indicates the error introduced by the multipath effect into pseudorange measurement. Indicates the error of the satellite navigation system. Indicates pseudorange, Indicates receiver clock bias. This indicates the error caused by multiple paths.

3. The method for optimizing the error model of multi-source heterogeneous navigation sources according to claim 1, characterized in that, In the error model of the inertial navigation subsystem, the output relationship of the gyroscope component of the inertial navigation system is expressed as follows: ； Among them, the actual angular velocity output of the gyroscope component Includes scaling factor error , This is the sensitivity coefficient. Angular velocity, zero-point drift and random colored noise ;including colored noise Modeled as random constants and first-order Markov processes The sum is as follows: ； in, It is a first-order Markov process; The output relationship of the accelerometer component in inertial navigation is expressed as follows: ； Actual acceleration output of the acceleration component The same model is used in China. , For constant drift, It is random colored noise.

4. The method for optimizing the error model of multi-source heterogeneous navigation sources according to claim 1, characterized in that, In the error model of the lidar subsystem, the lidar uses the time difference method to obtain distance information. The transmitting part emits a laser beam towards the target object. When the laser beam reaches the surface of the target object, it is reflected and received by the receiving part. By using the time difference between the transmitted and received pulses, the relative distance between the target point and the lidar is measured. The specific formula is as follows: ； c represents the speed of light, t represents time, and the scan point position information is expressed in spherical coordinates (r, ...). , Record the data based on the distance r from the scan point and the pitch angle. and azimuth Convert it to the Cartesian coordinate system of the lidar carrier itself.

5. The method for optimizing the error model of multi-source heterogeneous navigation sources according to claim 1, characterized in that, In the visual navigation subsystem error model, the relative position measured visually is modeled as follows: ； This represents the relative position output by the visual simulator in two adjacent frames. For the relative position truth value, It is Gaussian white noise.

6. The method for optimizing the error model of multi-source heterogeneous navigation sources according to claim 1, 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 as follows: , and ;in , The combined force Always pointing north, D is the magnetic heading, represented as: ； Since the magnetic heading angle ranges from 0° to 360°, while the experimentally calculated arctangent function ranges from -90° to 90°, and... To eliminate singular values, the calculation of magnetic heading was constrained, as follows: ； The magnetometer is affected by its own error and external magnetic field interference. Its measurement model is established as follows: ； The sensitivity error matrix is... Non-orthogonal error, For soft magnetic interference matrix, This is the hard magnetic interference vector. To achieve zero bias in the magnetometer, The Gaussian noise of the sensor, The actual magnetic field vector value, This represents the measured magnetic field vector value.

7. The method for optimizing the error model of multi-source heterogeneous navigation sources according to claim 1, characterized in that, In the UWB ranging subsystem error model, adjacent unmanned vehicles in the cluster assist in localization by receiving cooperative information from adjacent unmanned vehicles within the ranging range. Under good line-of-sight conditions, the inter-vehicle ranging value modeled based on UWB is as follows: ； For actual distance measurement between machines, The ranging 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: ； Among them, the position measurement value is The position measurement value of the i-th vehicle's adjacent node is Expanding the above equation using Taylor series, we get: ； In the formula The location of adjacent driverless vehicles. Indicates noise; error That is, the model is as follows: ； Further simplification yields: ； UWB error between nodes middle Part of it is the equivalent ranging noise that combines the position error of node i and the UWB ranging noise, with the noise variance being: ； In the formula Let be the position covariance of the i-th node.

8. A simulation system for error models of multi-source heterogeneous navigation sources, characterized in that, The simulation system is used to implement the method for optimizing the error model of multi-source heterogeneous navigation sources according to any one of claims 1-7. The system includes six types of sensor subsystem error models, error fusion and propagation models, selection models, and fusion simulation models. Based on the optimization results obtained from the multi-source heterogeneous navigation source error model optimization method, the optimized subsystem error model parameters are obtained. Then, the subsystem error models are fused by Kalman filtering to obtain the fused cooperative positioning error model. The specific steps are as follows: To address the fusion of multi-source PNT information in unmanned swarms under simulation rejection conditions, the continuous-time Kalman filter is discretized. The discretized system state equation and measurement equation are as follows: ； ； In the formula ； ； In the formula, For the iteration period, This represents the n-dimensional system state vector at time k. Represents the m-dimensional measurement vector at time k; This represents the one-step transition matrix of the system from time k-1 to time k. This represents the system noise at time k-1. Here is the system noise matrix. Represents the system measurement matrix; The system measurement noise is represented by the subscript k, which indicates the time series, F represents the state equation, and G represents the random noise variable. For the fusion of multi-source PNT information in unmanned swarms under simulation rejection conditions, the recursive equation modeled by discrete Kalman filtering is as follows: ； The first two terms are the state prediction equation and the covariance prediction equation, the middle term is the filter gain equation, and the last two terms are the updated state estimation equation and the covariance estimation equation; thus, the final fused error covariance can be solved. This enables simulation analysis of multi-source heterogeneous navigation.

Images