CN106197408A - A kind of multi-source navigation data fusion method based on factor graph - Google Patents

Abstract

The invention discloses a kind of multi-source navigation data fusion method based on factor graph, first pass through track and set the track obtaining meeting actual environment and mission requirements, sensor measurement information is obtained by various sensors, inertial navigation IMU data are obtained by inertia measurement unit, merge framework by factor graph structure multi-source navigation information, obtain navigation information eventually through multi-source information filtering fusion.Factor graph is combined by the present invention with Multi-source Information Fusion algorithm, it is achieved the process to asynchronous measurement information and the demand to navigation accuracy.

Description

A kind of multi-source navigation data fusion method based on factor graph

Technical field

The invention belongs to integrated navigation technology field, merge particularly to a kind of multi-source navigation information based on factor graph Method.

Background technology

Along with the development of science and technology, people's state to dynamic carrier moving target (aircraft, vehicle, naval vessel etc.) Estimated accuracy requires more and more higher, and dependence single-sensor carries out navigating and can not meet demand, then occurs in that multisensor Integrated navigation.Multi-sensor information fusion is arisen at the historic moment, and the information that multisensor provides is carried out by certain fusion criterion Excellent fusion, improves precision of state estimation.Merge the development of navigation based on navigation sensor.In integrated navigation system, often Sensing system include: inertial navigation system, satellite navigation system, celestial navigation system, Doppler naviga tion system, landform Matching Navigation System, scene matching navigation, dead reckoning navigation etc..The navigation principle used due to each sensor is different, respectively Extremely strong complementarity is there is between class sensor.In actual applications, owing to the renewal frequency of different sensors is different, in the presence of Between nonsynchronous problem, simultaneously in anabolic process, each sensor varying environment and under the conditions of use limitation, availability meeting Changing, influence whether filter structure, such as, satellite navigation signals is difficult to penetrated surface and the bigger thing of building isodensity Matter, in city, indoor, underground environment, the relaxation phenomenon of signal is very serious, causes GPS to be difficult to normally and works.Use fixing Filter structure and method all can not meet this application demand complicated and changeable well.

Probability graph model be a kind of with graph model to represent the theory of variable probabilistic dependency relation.Probability graph model can divide It is three classes, is oriented probability graph model, undirected probability graph model and mixing probability graph model respectively；Wherein, oriented probability artwork Type can be divided into again unidirectional figure and two-dimensional plot.Factor graph (Factor Graph, FG) is a kind of two-way probability graph model, comprises shellfish The multiple graph models such as this network of leaf, markov random file and Tanner figure.Figure comprises two kinds of node: one is to become Amount node, represents the variable in the overall situation function of many variables；One is factor nodes, represents the local functions in factorization.Each Local functions is only relevant to the Partial Variable in the overall situation function of many variables, and if only if variable is local functions when argument, because of There is one between variable node corresponding in subgraph and factor nodes and be connected limit.Factor graph is as a kind of problem analysis Graphical tool is obtained for extensively application in a lot of fields, such as: signal processing, artificial intelligence, neutral net etc..

Factor graph, as a kind of graph model instrument, has a wide range of applications at codec domain, meanwhile, is also applied to system Meter, signal processing and artificial intelligence field.Have document to channel estimation and equalization based on factor graph and the iteration of decoding and Studied；Also document utilization factor graph is had to propose a kind of multi-destination ground map generalization method.But it is generally speaking, the most right The application of factor graph is concentrated mainly on communication decoding field, to the research combined with Multi-source Information Fusion algorithm by factor graph Seldom.

Summary of the invention

In order to solve the technical problem that above-mentioned background technology proposes, it is desirable to provide a kind of multi-source based on factor graph Navigation data fusion method, combines factor graph with Multi-source Information Fusion algorithm, it is achieved the place to asynchronous measurement information Reason and the demand to navigation accuracy.

In order to realize above-mentioned technical purpose, the technical scheme is that

A kind of multi-source navigation data fusion method based on factor graph, comprises the following steps:

(1) under airborne dynamic environment, according to practical situation, mission requirements and the local environment design vector of carrier Athletic performance, movement locus, and determine with Inertial Measurement Unit be combined navigation other kinds airborne high-precision sensor；

(2) angular velocity information and the specific force letter of carrier is obtained by the gyroscope in Inertial Measurement Unit and accelerometer Breath, meanwhile, obtains all kinds of measurement information of carrier by other kinds airborne high-precision sensor；

(3) definition navigation system state vector is the variable node of factor graph, defines Inertial Measurement Unit and other are each The factor nodes that carrier measurement information is factor graph that the airborne high-precision sensor of class obtains, thus build many based on factor graph Source navigation information merges framework；

(4) under multi-source navigation information based on factor graph merges framework, sign system state vector and measurement information are more New process, sets up filtering equations, estimates through Real-Time Filtering and revises, completing the effective integration of multi-source navigation information.

Further, the detailed process of step (4) is as follows:

A () selects system state amount X, build state equation and the measurement equation of navigation system；

(b) choose multi-source navigation information merge constraint rule:

$P\left(X\right)=\underset{n\∈N}{\Π}{f}_n\left(X_k\right)---\left(1\right)$

In formula (1), P (X) is the joint distribution function being defined in system state amount X, f_n(X_k) be factor nodes, N be because of Son node number, X_kSystem state amount for the k moment；

C () sets up the factor nodes expression formula of Inertial Measurement Unit, set up other kinds airborne high-precision sensor because of Child node expression formula；

The cost function of (d) selective factor B node, and when its value takes minimum, system state amount X is sought partial derivative, thus Obtain the estimation of quantity of state X.

Further, in step (c), the expression formula of Inertial Measurement Unit factor nodes:

f_IMU=L (X_k+1-F(X_k,Z_IMU)) (2)

The factor nodes expression formula of other kinds airborne high-precision sensor:

f(X_k)=L (Z_k-H) (3)

Wherein, L () is cost function, and H is to measure function,It is to X_kEstimation, Z_kFor actual amount measured value, F is to be The transfer function matrix of system, X_k+1For the quantity of state of etching system, Z during k+1_IMUMeasuring value Z for Inertial Measurement Unit_IMU={ f_b, ω_b, f_b、ω_bIt is respectively specific force and angular velocity that inertial sensor measurement obtains.

Further, the specifically comprising the following steps that of step (d)

The cost function of selective factor B node also makes its value minimum:

$L(Z-H)={(Z-H\hat{X})}^{T}W(Z-H\hat{X})=min---\left(4\right)$

Formula (4) is asked the partial derivative of quantity of state X, and to make it is 0:

$\frac{\∂L}{\∂X}{|}_{X=\hat{X}}=-H^T(W+W^T)(Z-H\hat{X})=0---\left(5\right)$

The estimation to system state amount X is obtained according to formula (5):

$\hat{X}={\left(H^{T}WH\right)}^{-1}{H}^{T}WZ---\left(6\right)$

Wherein, W is positive definite weighting matrix.

Further, step (a) selects system state amount

Wherein, φ_E, φ_N, φ_UFor platform error angle, δ V_E, δ V_N, δ V_UThe velocity error in direction, sky, northeast, δ L, δ λ, δ h latitude Degree, longitude, height and position error, ε_bx, ε_by, ε_bzFor gyroscope arbitrary constant, ε_rx, ε_ry, ε_rzFor gyroscope single order markov Process,_x,_y,_zAccelerometer first-order Markov process.

The beneficial effect that employing technique scheme is brought:

(1) present invention uses factor graph to configure different navigation system, for the different application feature of sensor Design software framework, sets up the filtering framework being applicable to asynchronous isomery navigation system.Actual performance according to multi-source navigation system Characterize and fault characteristic, the self-adapting reconstruction scheme of research filtering framework, improve the precision and fault-tolerant of navigation system entirety filtering Performance；

(2) present invention analyzes the space and time difference of each sensor measurement information, on the basis of multi-source information Intelligent Fusion structure On, the sensor of different update rate, different error condition is abstracted into merit, merges for the aggregators design overall situation and calculate Method.

Accompanying drawing explanation

Fig. 1 is the overview flow chart of the present invention；

Fig. 2 is the exemplary plot of factor graph；

Fig. 3 is the exemplary plot of markovian factor graph；

Fig. 4 is the factor graph that in embodiment, five class sensors are constituted.

Detailed description of the invention

Below with reference to accompanying drawing, technical scheme is described in detail.

The substantially flow process of the present invention as shown in Figure 1, first passes through track setting and obtains meeting actual environment and mission requirements Track, various sensors obtain sensor measurement information, inertia measurement unit obtain inertial navigation IMU data, pass through the factor Figure structure multi-source navigation information merges framework, on this basis, obtains navigation information eventually through multi-source information filtering fusion.Hereafter The whole process of the present invention will be described in detail.

1, under airborne dynamic environment, according to practical situation, mission requirements and the local environment design vector of carrier Athletic performance and movement locus.

As a example by this sentences aircraft, the design to flight path is specifically described.Under airborne dynamic environment, to design and meet Complex environment residing for aircraft flight demand, carrier and the flight path of task feature.For different types of aircraft, example For: aircraft (such as: fighter plane) that mobility is stronger and the more weak aircraft (such as: transporter) of mobility, its working environment, fly Row maneuver, mission requirements are also very different, and severe degree is the most different.For fixed wing airplane, typically Flare maneuver can be divided into the most several: 1. puts down and flies and turn；2. spiral；3. spiral turning；4. somersault, S-shaped and 8-shaped；⑤ Let down motor-driven；6. rolling；7. the combination of straight line；8. tail is sliding motor-driven.

The performance of different system to be measured, flight maneuver can select one or more to be combined in above-mentioned action. According to above-mentioned requirements, design a flight path tallied with the actual situation so that it is mission requirements can be met and be actually needed.

2, angular velocity information and the ratio force information of carrier is obtained by the gyroscope in Inertial Measurement Unit and accelerometer, Meanwhile, all kinds of measurement information of carrier is obtained by other kinds airborne high-precision sensor.

Factor graph framework be a kind of have fast integration and reconfigure any navigation sensor and the framework of sensing element, Abstract method and filtering method.Wherein, can be used for the navigation elements numerous types of factor graph framework, inclination compass, distance/pseudorange Diastimeter, barometer, temperature sensor, azimuth rate sensor, laser radar, accelerometer, gyroscope, magnetometer, meter Time device, pedometer, star sensor, infrared sensor, polarized light sensor, X-ray detector, light flow sensor etc..According to appointing Business demand and practical situation, select sensor.

3, the foundation of Multi-source Information Fusion framework

Definition navigation system state vector is the variable node of factor graph, definition Inertial Measurement Unit and other kinds machine Carry the factor nodes that carrier measurement information is factor graph that high-precision sensor obtains, thus build multi-source based on factor graph and lead Boat information fusion framework.

Factor graph is bipartite model G=(F, X, E) representing navigation estimation problem, wherein, comprises two kinds of joint Point: one is factor nodes f_i∈ F, represents the local functions in factorization；One is variable node x_j∈ X, represents the overall situation many Variable in meta-function.Edge e_ij∈ E refers to, state variable nodes x in and if only if factor graph_jWith corresponding factor joint Point f_iTime relevant, there is one between them and connect limit.

Assume g (x₁,...,x_n) it is the product of several local functions, the parameter of each local functions is included in subset {x₁,...,x_nIn }, such as:

$g(x_1,...,x_n)=\underset{j\∈J}{\Π}{f}_j\left(X_j\right)---\left(1\right)$

Wherein, J is discrete indicator collection, X_jIt is { x₁,...,x_nSubset, f_j(X_j) it is a function, parameter is X_j。

Factor graph is i.e. a bipartite graph, is used for describing the structure of (1) formula factorization.Make g (x₁,x₂,x₃,x₄,x₅) make For 5 variablees of function, if g can be to be represented as the form of 5 factor products:

g(x₁,x₂,x₃,x₄,x₅)=f_A(x₁)f_B(x₂)f_C(x₁,x₂,x₃)f_D(x₃,x₄)f_E(x₃,x₅) (2)

So, J={A, B, C, D, E}, X_A={ x₁, X_B={ x₂, X_C={ x₁,x₂,x₃, X_D={ x₃,x₄, X_E={ x₃, x₅}.The factor graph of formula (2) correspondence is as shown in Figure 2.

Probabilistic model is a main application direction of factor graph.Such as, if X, Y, Z are markovian for forming Stochastic variable, their joint probability density

p_XYZ(x, y, z)=p_X(x)p_Y|X(y|x)p_Z|Y(z|y) (3)

The factor graph of this expression formula represents as shown in Figure 3.

In the present invention, select inertial navigation (Strapdown Inertial Navigation System, SINS), astronomy Navigation (Celestial Navigation System, CNS), terrain contour matching (Terrain Contour Matching, TERCOM), synthetic aperture radar (Synthetic Aperture Radar, SAR) scene matching aided navigation carries out information fusion, and uses gas Pressure altimeter (Barometric altimeter) highly assists, and as example, factor graph is merged framework and illustrates.

By abstract to SINS, CNS, TERCOM, SAR, Barometer be five factor nodes, and multielement bar is merged Navigation framework factor graph structure shows, as shown in Figure 4.In figure, circle represents state variable nodes, and black bars represents Factor nodes, X_kRepresenting the navigational state of system, f represents each sensor measurement information.Prior represents previous measurement information, f_IMURepresent the measurement information from IMU, with t_kMoment and t_k+1The navigational state in moment is correlated with, f_CNS、f_TERCOM、f_SAR、 f_BarometerIt is the measurement information from CNS, TERCOM, SAR, Barometer the most respectively.

4, multi-source navigation information based on factor graph merges

Under the Open architecture framework of factor graph, characterize the state of system and measure renewal process, setting up filtering equations, Estimate through Real-Time Filtering and revise, thus completing the effective integration of Multiple Source Sensor information, it is achieved the plug and play of sensor.

The state equation of system is maintained in the 18 of navigation system:

$\stackrel{\·}{X}\left(t\right)=A\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)---\left(4\right)$

In formula: X (t) is state vector；A (t) is coefficient of regime matrix；G (t) is error coefficient matrix；W (t) is white noise Sound random error vector.

System state vector X is:

Contain basic navigation parameter error and the error state amount of 9 dimension inertia type instruments of 9 dimension inertial navigation systems.Wherein, φ_E, φ_N, φ_UFor platform error angle, δ V_E, δ V_N, δ V_UThe velocity error in direction, sky, northeast, δ L, δ λ, δ h latitude, longitude, highly Site error, ε_bx, ε_by, ε_bzFor gyroscope arbitrary constant, ε_rx, ε_ry, ε_rzFor gyroscope first-order Markov process,_x,_y, ▽_zAccelerometer first-order Markov process.

Multi-source Information Fusion algorithm based on factor graph is intended to find a solution that disclosure satisfy that constraint rule, therefore, needs Constrained rule is wanted to carry out constraint solving process.Here, constraint is chosen for by we

$P\left(X\right)=\underset{n\∈N}{\Π}{f}_n\left(X_k\right)---\left(6\right)$

In formula (6), P (X) is the joint distribution function being defined on X.One merit node can be write as f (X_k)= L(Z_k-H) form, represent factor nodes obtain prediction measurement information and the difference of actual amount measurement information, build accordingly finger Scalar functions thus obtain the estimation of state variable.L () is the cost function of the amount of being estimated；H, for measuring function, is to become with state The function that amount is relevant, in navigation framework, H can predict the measuring value of sensor according to given state estimation；Z_kIt is by respectively The actual amount measured value that class sensor obtains.

IMU node in factor nodes is different from other measurement node, its measuring value Z_IMUValuation X with the k moment_kQuilt It is used for predicting value X in k+1 moment_k+1, measuring value Z_IMUExpression formula be

Z_IMU={ f_b,ω_b} (7)

Wherein, f_b、ω_bIt is respectively specific force and angular velocity that inertial sensor measurement obtains, then can get IMU factor nodes Expression formula

f_IMU=L (X_k+1-F(X_k,Z_IMU)) (8)

Wherein, F is system transter matrix, X_k+1For the state vector of etching system during k+1.

The cost function L of selective factor B node, and make its value take minimum, obtain

$L(Z-H)={(Z-H\hat{X})}^{T}W(Z-H\hat{X})=min---\left(9\right)$

Wherein, W is the positive definite weighting matrix of suitable value,It it is the estimation to quantity of state X.Above formula to be made is set up,Should Meet

$\frac{\∂L}{\∂X}{|}_{X=\hat{X}}=-H^T(W+W^T)(Z-H\hat{X})=0---\left(10\right)$

Thus obtain currently state X being estimated as

$\hat{X}={\left(H^{T}WH\right)}^{-1}{H}^{T}WZ---\left(11\right)$

Data from asynchronous heterogeneous sensor can be entered by based on factor graph multi-source navigation data fusion method easily Row processes, and after receiving the output data of sensor, expansion factor node of graph, state equation and measurement equation according to system are fast Speed effectively carries out the renewal of system mode, it is achieved the aggregation of data of multisensor processes, and is effectively improved the reliable of navigation system Property and the ability of rapid configuration, be greatly improved navigation performance.

Above example is only the technological thought that the present invention is described, it is impossible to limit protection scope of the present invention with this, every The technological thought proposed according to the present invention, any change done on the basis of technical scheme, each fall within scope Within.

Claims (5)

1. a multi-source navigation data fusion method based on factor graph, it is characterised in that comprise the following steps:

(1) under airborne dynamic environment, according to practical situation, mission requirements and the motion of local environment design vector of carrier Action, movement locus, and determine with Inertial Measurement Unit be combined navigation other kinds airborne high-precision sensor；

(2) angular velocity information and the ratio force information of carrier is obtained by the gyroscope in Inertial Measurement Unit and accelerometer, with Time, obtain all kinds of measurement information of carrier by other kinds airborne high-precision sensor；

(3) definition navigation system state vector is the variable node of factor graph, definition Inertial Measurement Unit and other kinds machine Carry the factor nodes that carrier measurement information is factor graph that high-precision sensor obtains, thus build multi-source based on factor graph and lead Boat information fusion framework；

(4) under multi-source navigation information based on factor graph merges framework, characterize system state vector and measurement information is updated Journey, sets up filtering equations, estimates through Real-Time Filtering and revises, completing the effective integration of multi-source navigation information.

Multi-source navigation data fusion method based on factor graph the most according to claim 1, it is characterised in that step (4) Detailed process is as follows:

A () selects system state amount X, build state equation and the measurement equation of navigation system；

(b) choose multi-source navigation information merge constraint rule:

$P\left(X\right)=\underset{n\∈N}{\Π}{f}_n\left(X_k\right)---\left(1\right)$

In formula (1), P (X) is the joint distribution function being defined in system state amount X, f_n(X_k) it is factor nodes, N is factor joint Count, X_kSystem state amount for the k moment；

C () sets up the factor nodes expression formula of Inertial Measurement Unit, set up the factor joint of other kinds airborne high-precision sensor Point expression formula；

The cost function of (d) selective factor B node, and when its value takes minimum, quantity of state X is sought partial derivative, thus obtain state The estimation of amount X.

Multi-source navigation data fusion method based on factor graph the most according to claim 2, it is characterised in that in step (c) In, the expression formula of Inertial Measurement Unit factor nodes:

f_IMU=L (X_k+1-F(X_k,Z_IMU)) (2)

The factor nodes expression formula of other kinds airborne high-precision sensor:

f(X_k)=L (Z_k-H) (3)

Wherein, L () is cost function, and H is to measure function,It is to X_kEstimation, Z_kFor actual amount measured value, F is the biography of system Delivery function matrix, X_k+1For the quantity of state of etching system, Z during k+1_IMUMeasuring value Z for Inertial Measurement Unit_IMU={ f_b,ω_b, f_b、 ω_bIt is respectively specific force and angular velocity that inertial sensor measurement obtains.

Multi-source navigation data fusion method based on factor graph the most according to claim 3, it is characterised in that step (d) Specifically comprise the following steps that

The cost function of selective factor B node also makes its value minimum:

$L(Z-H)={(Z-H\hat{X})}^{T}W(Z-H\hat{X})=min---\left(4\right)$

Formula (4) is asked the partial derivative of quantity of state X, and to make it is 0:

$\frac{\∂L(Z-H)}{\∂X}{|}_{X=\hat{X}}=-H^T(W+W^T)(Z-H\hat{X})=0---\left(5\right)$

The estimation to system state amount X is obtained according to formula (5):

$\hat{X}={\left(H^{T}WH\right)}^{-1}{H}^{T}WZ---\left(6\right)$

Wherein, W is positive definite weighting matrix.

Multi-source navigation data fusion method based on factor graph the most according to claim 2, it is characterised in that: in step (a) Select system state amount

Wherein, φ_E, φ_N, φ_UFor platform error angle, δ V_E, δ V_N, δ V_UThe velocity error in direction, sky, northeast, δ L, δ λ, δ h latitude, Longitude, height and position error, ε_bx, ε_by, ε_bzFor gyroscope arbitrary constant, ε_rx, ε_ry, ε_rzFor gyroscope single order markov mistake Journey,Accelerometer first-order Markov process.