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

CN119226711BCN 119226711 BCN119226711 BCN 119226711BCN-119226711-B

Abstract

The invention discloses a multisource heterogeneous navigation source error model optimization method and a simulation system, which are applied to an unmanned equipment navigation system and comprise the following steps of S1, calculating each sensor subsystem error model; S2, initial calibration, S3, primary screening of a sensor subsystem, S4, secondary screening of a sensor, S5, simulation of motion parameters, S6, judgment of whether the sensor is more than or equal to 2 after optimization, simulation analysis, kalman filtering correction and storage of simulation data if the sensor is more than or equal to 2, direct storage of the prevention data if the sensor is more than or equal to 2, S7, judgment of whether the simulation is finished if the simulation is finished, and if the simulation is finished, the step S8 is started, the step S3 is returned, and the step S8 is drawn. The simulation model can be compatible with various information sources at the same time, builds a corresponding navigation model aiming at different information sources, can flexibly select the model according to the information sources, can flexibly select different matrix types according to different actual use environments, and has the advantages of stronger compatibility, wider coverage, higher flexibility and better environment adaptability.

Inventors

ZHANG RUICHEN
FAN GUANGTENG
CAO LU
ZHANG FEI
WU PENG
QIN JIANGYI
WANG KAI

Assignees

中国人民解放军军事科学院国防科技创新研究院

Dates

Publication Date: 20260508
Application Date: 20241129

Claims (8)

1. The multi-source heterogeneous navigation source error model optimization method is applied to an unmanned equipment navigation system and is characterized in that the multi-source heterogeneous navigation source comprises inertial information output by an inertial navigation subsystem, longitude and latitude height information output by a satellite navigation subsystem, a visual mileage scoring system, a laser radar mileage scoring system, relative measurement navigation information output by a UWB subsystem and course angle information output by a magnetic force scoring system, and the method comprises the following steps: S1, calculating an error model of each sensor subsystem; s2, initial calibration of each sensor; S3, primary screening of a sensor subsystem, setting an I-type index as error covariance, and respectively calculating six types of subsystem error covariance, wherein PNT sources with the error covariance larger than an error threshold value are not in a consideration range; s4, secondary screening of the sensor, namely drawing respective performance radar graphs of other subsystem models meeting the error threshold value requirement according to five aspects of class II indexes respectively, calculating areas in the respective performance radar graphs, sorting from large to small according to the areas, screening out the first three PNT sources as final preferable results, and reserving the first two PNT sources; S5, simulating motion parameters, namely simulating the motion trail of the unmanned platform and corresponding parameters thereof through a flight path generator; S6, judging whether the sensor is more than or equal to 2 after optimization, if so, performing simulation analysis, kalman filtering correction and saving simulation data, and if not, directly saving the simulation data; s7, judging whether the simulation is finished, if so, entering a step S8, and if not, returning to the step S3; S8, drawing a change curve of fusion errors along with time; in the step S1, the sensor subsystem error model comprises a satellite navigation subsystem error model, an inertial navigation subsystem error model, a laser radar subsystem error model, a visual navigation subsystem error model, a magnetic scoring system error model and a UWB ranging subsystem error model; The process of the secondary screening of the six subsystem error models through the step S4 is as follows: S4.1 setting II type indexes, and carrying out error covariance and error controllability Error influencing factors, error propagation mechanism and positioning continuity Setting class II indexes in five aspects, and evaluating by means of a radar chart; S4.2, setting a class II index threshold value, namely determining the class II index threshold value according to the specific environment complexity and the specific requirements; And S4.3, secondary screening, namely drawing respective performance radar graphs of subsystem error models meeting the threshold value requirement according to five aspects of class II indexes, calculating the area in the respective performance radar graphs, sorting according to the area from large to small, screening out the first three PNT sources as final preferred results, and reserving the first two PNT sources with less than three PNT sources.
2. The method for optimizing a multi-source heterogeneous navigation source error model according to claim 1, wherein in the satellite navigation subsystem error model, the error modeling is as follows: ; , , , Respectively represent the navigation message error, satellite clock error, ionosphere time delay and troposphere time delay The pseudo-range error of a satellite is determined, In order to achieve the light velocity, the light beam is, Representing the noise of the measurement, Representing errors in the pseudorange measurements due to multipath effects, Representing the error of the satellite navigation system, The pseudo-range is represented and, Indicating the receiver clock-difference and, Indicating errors due to multipath.
3. The method for optimizing a multi-source heterogeneous navigation source error model according to claim 1, wherein in the inertial navigation subsystem error model, the output relation of a gyro component of inertial navigation is expressed as: ; wherein the actual angular velocity output of the gyro assembly Including scale factor errors , As a coefficient of sensitivity, a reference number, Zero drift for angular velocity And random colored noise Wherein the color noise Modeling as a random constant And a first order Markov process The sum is as follows: ; Wherein, the Is a first order markov process; The accelerometer component output relationship for inertial navigation is expressed as: ; actual acceleration output of acceleration component In the same way modeled as , Is a constant value of the drift and, Is random colored noise.
4. The method according to claim 1, wherein in the error model of the multi-source heterogeneous navigation source, the laser radar obtains distance information by using a time difference method, the transmitting part transmits a laser beam to the target object, the laser beam is reflected and received by the receiving part when reaching the surface of the target object, and the relative distance measurement between the target point and the laser radar is completed by the time difference between the transmitting and receiving two pulses, and the specific formula is as follows: ; c represents the speed of light, t represents time, and the scan point position information is represented in spherical coordinates (r, , ) Recording according to the distance r and pitch angle of the scanning point And azimuth angle And converting the laser radar into a rectangular coordinate system of a laser radar carrier.
5. The method of claim 1, wherein modeling the relative position of the visual measurement in the visual navigation subsystem error model is: ; for the relative position of the visual simulator output during the time of two adjacent frames, Is a true value of the relative position, Is white gaussian noise.
6. The method for optimizing a multi-source heterogeneous navigation source error model according to claim 1, wherein in the error model of the magnetometry system, three-axis components of the vector H measured by the magnetometers under the geographic coordinate system are respectively: 、 And Wherein 、 Is of the total force of (2) Always refer to north orientation, D is magnetic heading, expressed as: ; The magnetic heading angle is 0-360 degrees, and the value range of the arctangent function calculated by experiments is (-90 degrees, 90 degrees), and In order to eliminate singular values, the calculation of the magnetic heading is constrained, specifically as follows: ; the magnetometer is influenced by own error and external magnetic field interference factors, and a measurement model is established as follows: ; in order to be a sensitivity error matrix, Is a non-orthogonal error that is not a true error, In the form of a soft magnetic interference matrix, As a vector of the hard magnetic disturbance, Is the zero offset of the magnetometer, As a gaussian noise of the sensor, The true magnetic field vector value is calculated, Indicating the measured magnetic field vector value.
7. The method for optimizing a multi-source heterogeneous navigation source error model according to claim 1, wherein in the UWB ranging subsystem error model, two adjacent unmanned vehicles in a cluster assist in positioning by receiving cooperative information of the adjacent unmanned vehicles within a ranging range, and the inter-machine ranging value modeling based on UWB under a condition of good viewing distance is as follows: ; for the real ranging between the machines, For the ranging information output by the sensor, Representing noise if the current position is The position of the ith adjacent node is The UWB output is then: ; Wherein the position measurement is The position measurement value of the ith adjacent node is The taylor expansion of the above is: ; In the middle of Is the position of the adjacent unmanned vehicle, Representing noise; Error of (2) Namely, modeling is as follows: ; Further simplifying and obtaining: ; UWB error between nodes In (a) The equivalent ranging noise combining the position error of the node i and the UWB ranging noise is the noise variance: ; In the middle of Is the positional covariance of the i-th node.
8. The multi-source heterogeneous navigation source error model simulation system is characterized in that the simulation system is used for realizing the multi-source heterogeneous navigation source error model optimization method according to any one of claims 1-7, and the system comprises six sensor subsystem error models, an error fusion transfer model, a selection model and a fusion simulation model; Based on a preferred result obtained by a multisource heterogeneous navigation source error model preferred method, preferred subsystem error model parameters are obtained, and fusion is carried out on the subsystem error models through Kalman filtering, so that a fused co-location error model is obtained, and the method comprises the following specific steps: Aiming at unmanned cluster multisource PNT information fusion under the simulation rejection condition, discretizing continuous time domain Kalman filtering, wherein a discretized system state equation and a discretized measurement equation are respectively as follows: ; ; In the middle of ; ; In the formula, For the period of the iteration, An n-dimensional system state vector representing time k, Representing an m-dimensional measurement vector at the moment k; representing a one-step transition matrix of the system from time k-1 to time k, Representing the system noise at time k-1, In the form of a system noise matrix, Representing a system measurement matrix; The method is characterized in that the method comprises the steps of representing system measurement noise, subscript k represents a time sequence, F represents a state equation, and G represents a random noise variable; Aiming at unmanned cluster multisource PNT information fusion under a simulation rejection condition, a recursive equation modeling of discrete Kalman filtering is as follows: ; Wherein the first two terms are a state prediction equation and a covariance prediction equation, the middle is a filtering gain equation, and the last two terms are an updated state estimation equation and a covariance estimation equation, so as to solve the final fused error covariance And realizing simulation analysis of multi-source heterogeneous navigation.

Description

Multi-source heterogeneous navigation source error model optimization method and simulation system Technical Field The invention relates to unmanned technology and navigation technology, in particular to a multisource heterogeneous navigation source error model optimization method and a simulation system. Background Along with the development of the times, unmanned technology has become the current mainstream development trend, unmanned vehicles, unmanned aerial vehicles and the like develop rapidly, and meanwhile, higher requirements are also provided for positioning navigation technology. At present, the technology of UWB, LTE, bluetooth, WIFI and the like is mainly adopted for positioning indoors, and the technology of UWB, LTE, bluetooth, WIFI and the like is mainly adopted for navigation, inertial navigation, pseudolite navigation and the like outdoors. In recent years, unmanned technology is rapidly rising, and a position service is popularized in a large area, and meanwhile, higher requirements are also provided for a reliable navigation system, so that a global satellite navigation system (GNSS) is a common outdoor positioning mode, but the requirements of the GNSS on the environment are extremely high, so that after GNSS satellite signals are shielded by shielding objects in a complex environment, the problems of reduced signal strength, increased noise, reduced precision, lost data and the like cause that the GNSS cannot provide accurate positioning, and at the moment, the availability, the continuity and the reliability of PNT service of the GNSS cannot be ensured. Because the satellite navigation system is easy to be interfered in the current common navigation technology and cannot be used in the indoor environment, the combined navigation or micro PNT and other methods are mainly adopted for positioning navigation, but even so, the navigation inevitably has error problems caused by various reasons, the current error analysis models aiming at different navigation modes are mostly incomplete, only 2-3 kinds of error analysis of the navigation modes can be supported, other navigation means cannot be effectively compatible, the navigation error models are relatively fixed, the selection cannot be carried out according to the factors such as the environment, and the flexibility is insufficient. The current PNT error analysis model is not flexible and rich enough, and most of the model is only suitable for 1-3 navigation modes, such as a global satellite positioning system and inertial navigation system combined navigation system, a sensor information fusion-based combined navigation system, a GPS combined navigation system and a Doppler combined navigation system, and the model is fixed and cannot change the cooperative mode according to the environment. The existing problems are mainly focused on the following three points: 1. The model is single, the application scope is small, the compatibility is poor, once the navigation mode is changed, the error model needs to be reconstructed, the time and the labor are wasted, and the development cost is increased. 2. The method has the advantages that the environment adaptability is poor, the error model analysis can only be carried out aiming at fixed environment parameters, once the environment changes, the accuracy can be seriously affected, the error model result is unstable and inaccurate, and the result has no reference value. 3. The flexibility is low, and the error fusion model cannot be changed according to actual requirements. Disclosure of Invention Aiming at the problems existing in the prior art, the invention aims to provide a multi-source heterogeneous navigation source error model optimization method and a simulation system, wherein the simulation model can be compatible with various information sources at the same time, a corresponding navigation model is built aiming at 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, compatibility is stronger, coverage is wider, flexibility is higher, environmental adaptation is better, and functions are strong. The invention provides a multi-source heterogeneous navigation source error model optimization method which is applied to an unmanned equipment navigation system and comprises inertial information output by an inertial navigation subsystem, longitude and latitude height information output by a satellite navigation subsystem, a visual mileage scoring system, a laser radar mileage scoring system, relative measurement navigation information output by a UWB subsystem and course angle information output by a magnetic force scoring system, wherein the method comprises the following steps: S1, calculating an error model of each sensor subsystem; s2, initial calibration of each sensor; S3, primary screening of a sensor subsystem, namely respective