CN115127591A - Airborne DPOS transfer alignment method based on statistical confidence distance measurement bootstrapping - Google Patents
Airborne DPOS transfer alignment method based on statistical confidence distance measurement bootstrapping Download PDFInfo
- Publication number
- CN115127591A CN115127591A CN202210746082.9A CN202210746082A CN115127591A CN 115127591 A CN115127591 A CN 115127591A CN 202210746082 A CN202210746082 A CN 202210746082A CN 115127591 A CN115127591 A CN 115127591A
- Authority
- CN
- China
- Prior art keywords
- node
- sub
- measurement
- child node
- fusion
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
Landscapes
- Engineering & Computer Science (AREA)
- Manufacturing & Machinery (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Navigation (AREA)
Abstract
An airborne DPOS transfer alignment method based on statistical confidence distance measurement bootstrapping. First, t is solved k The positions, speeds and postures of the main node and each sub node at any moment, and the redundant positions and postures of each sub node; then, using a measurement bootstrap strategy to generate t k Respectively sampling the virtual measurement vector of each virtual measurement set by using Markov chain Monte Carlo sampling method to obtain t k The fusion measurement set of each child node at the moment; secondly, calculating the mutual support degree of any two fusion measurement vectors in each fusion measurement set by using a consistency fusion method based on statistical confidence distance to obtain each sub-node t k The effective measurement vector of the moment and the noise matrix thereof; finally, establishing a transfer alignment model of each sub-node, carrying out transfer alignment based on Kalman filtering, and estimating t k The position error, the speed error and the attitude error of each subnode are obtained at any moment, and the motion parameters of each subnode are corrected to obtain more accurate motion parametersAnd (4) motion parameters.
Description
Technical Field
The invention relates to the field of navigation systems, in particular to an airborne Distributed POS (Distributed POS, DPOS) transfer alignment method based on statistical confidence distance measurement bootstrapping, which can be used for improving the estimation accuracy of airborne DPOS sub-node motion parameters.
Background
Airborne earth observation systems that integrate multiple or multiple remotely sensed loads have become one of the major trends in earth observation. Such as synthetic aperture radars, integrated mapping cameras, multispectral scanners, large-field infrared scanners, and the like. To realize high-precision imaging of the multi-task loads, high-precision motion parameters of each load mounting point need to be acquired.
The Position and attitude measurement System (POS) can provide accurate Position, speed, and attitude information for remote sensing loads, and is a key component for realizing airborne earth observation. The information such as position and attitude acquired by the POS system can be used for image correction or motion compensation, so that the imaging quality is improved. The POS mainly includes an Inertial Measurement Unit (IMU), a global navigation satellite System receiver, a POS Computer System (PCS), and post-processing software.
For an airborne aircraft equipped with an aerial remote sensing system with multiple or multiple observation loads, since the multiple or multiple observation loads are installed at different positions of the aircraft, the requirement of high-precision motion parameter measurement of the multiple loads cannot be met by continuously adopting a traditional single POS system, and therefore, a Distributed POS (Distributed POS) application capable of measuring multiple point motion parameters is generated to provide high-precision space-time information for all loads. The DPOS is generally composed of a high-precision master POS, a plurality of sub-IMUs, a PCS and post-processing software. The main POS is also called a main node and is generally positioned at the belly of the engine room to provide a high-precision space-time reference; the sub IMUs are also called as sub nodes and are often distributed and installed on wings on two sides, and the main node provides high-precision position, speed and posture information for the sub IMUs through transfer alignment, so that the accurate measurement of the motion information of each sub IMU is realized. It can be said that transfer alignment is one of the key technologies that improve the performance of DPOS.
Ideally, the measurement accuracy of the transfer alignment from the master POS to each of the sub-IMUs, respectively, should be consistent. However, in actual flight, due to differences of factors such as body deformation, lever arm errors and inertial device precision of the placement points of the sub-IMUs, the transfer alignment precision of the sub-IMUs is different. Generally speaking, the sub IMU close to the center of the machine body has small deflection deformation relative to the main POS, so that the transfer alignment precision is higher, and the precision requirement of imaging motion compensation is easily met; and the sub IMU far away from the center of the machine body has large flexural deformation relative to the main POS and complex conditions, so the transfer alignment precision is low, and the precision requirement of imaging motion compensation cannot be met. Therefore, each sub-IMU only carries out transfer alignment from the main POS to the sub-IMU, the obtained motion parameter precision cannot meet the overall precision requirement, the output information of all the sub-IMUs needs to be comprehensively utilized for data fusion, and the overall measurement precision of the distributed system is improved.
The DPOS can also be regarded as a multi-node inertial network system, and the data fusion methods for the multi-node inertial network system include a centralized fusion method, a distributed fusion method, a measurement bootstrap method based on a characteristic value, and the like. The output information of all the child nodes is calculated in a fusion center through centralized fusion, the information loss is small, and the method is globally optimal, but the method involves a large number of high-dimensional matrix inversion operations, so that the calculation amount is large, and the fault tolerance is low; in the distributed fusion, the original output information of all the sub-nodes is filtered in the filters of the respective sub-nodes, and then is subjected to centralized processing by the same fusion center, the method needs to calculate a weight matrix for data fusion, but the inversion of the matrix is involved in the calculation of the weight matrix, so that the total calculated amount is large, and the problem that the matrix is singular easily occurs; the bootstrap measurement fusion method based on the characteristic value measurement is characterized in that a bootstrap measurement set is constructed on the basis of output information of all sub-nodes, a resampling method is used for sampling the bootstrap measurement set, weights of bootstrap measurement are calculated for the sampled bootstrap measurement set through a consistency fusion method based on the characteristic value, then data fusion is carried out to obtain a weighted mean value to obtain an effective measurement vector, the adverse effect of uncertain measurement noise on the reliability of the bootstrap measurement set is improved, and state estimation precision is improved. For example, patent No. CN202110309944.7 adopts a bootstrap method based on eigenvalue measurement, which uses the effective measurement vector obtained by performing data fusion on bootstrap measurement as the measurement vector of kalman filtering. However, when the method performs data fusion on the bootstrap measurement set, the method is influenced by bootstrap measurement vectors with low mutual support degree, so that the error of the effective measurement vector obtained by final fusion is large, and the precision of transfer alignment is influenced.
Disclosure of Invention
The technical problem to be solved by the invention is as follows: the method can improve the transfer alignment precision of each sub-node in the airborne DPOS.
The technical solution of the invention is as follows: an airborne DPOS transfer alignment method based on statistical confidence distance measurement bootstrapping. The method comprises the following specific steps:
(1) resolving t k The positions, the speeds and the postures of the main node and each sub-node at the moment, and the redundant positions and the postures of each sub-node;
(2) generating t using a measurement bootstrapping strategy k A virtual measurement set of each child node at a moment;
(3) respectively sampling the virtual measurement vectors in each virtual measurement set by using a Markov chain Monte Carlo sampling method to obtain t k The fusion measurement set of each child node at the moment;
(4) calculating the mutual support degree of any two fusion measurement vectors in each fusion measurement set by using a consistency fusion method based on statistical confidence distance to obtain an effective measurement vector and a noise matrix of each sub-node tk moment;
(5) establishing a transfer alignment model of each child node;
(6) transfer alignment based on Kalman filtering is carried out to estimate t k The position error, the speed error and the attitude error of each sub-node are carried out at any moment, and the motion parameters of each sub-node are corrected;
(7)t k ＝t k + 1; and (4) repeatedly executing the steps (1) to (7) until the transfer alignment of all the child nodes at all the time is completed.
Solving for t in the step (1) described above k The method comprises the following steps of obtaining the positions, speeds and postures of a main node and each sub-node at a moment, and obtaining redundant positions and postures of each sub-node:
1) definition of coordinate system
a) Earth's center inertial coordinate system, i system and inertial system for short
The origin of the coordinate system is geocentric, x i And y i Axis in the equatorial plane of the earth, z i Axis points to the spring equinox, axis points to the earth polar axis, y is determined by the right hand rule i An axial direction;
b) terrestrial coordinate system, e system for short
The earth coordinate system is a coordinate system which is fixedly connected with the earth and rotates along with the earth, and approximately considers that the earth coordinate system rotates at the earth rotation angular rate omega relative to the inertial coordinate system ie Rotation, omega ie The angle is approximately equal to 15.04 degrees/h; the origin of the coordinate system is the center of the earth, z e Axis directed to earth polar axis, x e Axis through zero meridian, y being determined by the right hand rule e An axial direction;
c) carrier coordinate system, carrier system for short
Its origin of coordinates is the center of mass of the carrier, x b The axis pointing to the right along the transverse axis of the carrier, y b The axis pointing forwards along the longitudinal axis of the carrier, z b The axis points upward along the vertical axis of the carrier; for the main and sub-nodes, b, in DPOS s The carrier systems respectively represent a main node and an s-th sub-node, wherein s is 1,2, …, N and N are the number of the sub-nodes;
d) navigation coordinate system, navigation system for short
The navigation system is a coordinate system selected according to work requirements when navigation parameters are solved, the navigation system is taken as a northeast geographical coordinate system, the navigation system of the main node is represented by n, and the navigation system and the calculation navigation system of the s-th sub-node are respectively represented by n s And n' s Represents;
2) the main node and each sub-node carry out strapdown resolving
The main node and all the sub-nodes are solved through strapdown to respectively obtain the main node and all the sub-nodes t k Position of time [ L m λ m H m ] T And [ L s λ s H s ] T And attitude [ psi m θ m γ m ] T And [ psi s θ s γ s ] T Speed, velocity
And
wherein L is m 、λ m And H m Respectively representing the latitude, longitude and altitude of the master node, L s 、λ s And H s Respectively representing the latitude, longitude and altitude of the s-th child node; psi m 、θ m And gamma m Respectively representing the heading, pitch and roll angles of the master node,. psi s 、θ s And gamma s Respectively representing a course angle, a pitch angle and a roll angle of the s-th sub-node;
and
respectively representing the east, north and sky speeds of the master node,
and
respectively representing east, north and sky speeds of the s-th sub-node;
3) acquisition of redundant position and posture of each child node
The redundant position and posture of a certain sub-node can be calculated by the positions and postures of the other sub-nodes and the corresponding relative positions and relative angles by utilizing the relative positions and relative angles between the sub-nodes measured by the fiber bragg grating sensors arranged on the wings; that is, any child node can obtain N-1 sets of redundant position and attitude information.
a) Acquisition of redundant positions of child nodes
T measured by fiber grating sensor k At any moment, two sub-nodes s and s * Relative position of
Wherein
Respectively representing two child nodes s, s * Difference in latitude, difference in longitude, difference in altitude, s * ＝1,2,…,N,s≠s * ；
t k At the moment, the position obtained after the sub-node s is subjected to strapdown calculation is [ L ] s λ s H s ] T And the positions of the rest N-1 sub-nodes after strapdown calculation are expressed as
The N-1 sets of redundant location information at the child node s can be represented as:
by N-1 sets of redundant locations at the child node s
And the position [ L ] of the byte point obtained by strapdown solution s λ s H s ] T Then t at the child node s is obtained k N sets of positions of time, note
b) Acquisition of child node redundancy posture
T measured by fiber grating sensor k Any two sub-nodes s, s at any moment * ，s≠s * Relative angle of rotation of
Wherein
Respectively representing child nodes s * The carrier system of (a) and the carrier system of the child node s have relative rotation angles between the x-axis, the y-axis and the z-axis; by passing
Can calculate the child nodes s, s * Relative attitude matrix between carrier systems
The calculation method is as follows:
the attitude matrix between the carrier system and the computational navigation system obtained by strapdown calculation at the child node s is
The respective attitude matrix calculated by other N-1 child nodes through strapdown solution is
N-1 redundant attitude matrices at child node s
Can be expressed as:
wherein the content of the first and second substances,
computing navigation system to child node s for child node s * And calculating a transformation matrix between the navigation systems by the following method:
wherein the content of the first and second substances,
are child nodes s, respectively * And a transformation matrix between the earth coordinate system and each computed navigation system; l is s 、
And λ s 、
Respective sub-nodes s, s * Latitude and longitude of;
t k time of day, by the attitude matrix at the child node s
And N-1 redundant attitude matrices
N groups of attitude angles including a course angle, a pitch angle and a roll angle can be obtained through calculation; the calculation method is as follows:
will the attitude matrix
Is recorded as:
wherein, the first and the second end of the pipe are connected with each other,
as a matrix of postures
The elements of the row a and the column b, a and b are 1,2 and 3; the heading angle psi at the child node s s Angle of pitch theta s And roll angle γ s Principal value of (2)
Respectively as follows:
the value ranges of the course angle, the pitch angle and the roll angle are respectively defined as [0,2 pi ]]、
[-π,+π](ii) a Then the heading angle psi s Pitch angle θ s And roll angle γ s Are respectively determined by the following formula:
according to the calculation method of the attitude angle, N-1 redundant attitude matrixes at the position of the child node s can be calculated
Corresponding N-1 groups of attitude angle information; thereby, unite and consist of
The calculated 1 group of attitude angle information obtains t at the position of the child node s k N sets of attitude information of time, and is recorded as
Generating t by using the measurement bootstrap strategy in the step (2) k The virtual measurement set of each child node at a moment specifically comprises the following steps:
at t k At any time, for any subnode s, s is 1,2, …, N vector quantities of the subnode can be obtained
The expression is as follows:
wherein, the first and the second end of the pipe are connected with each other,
respectively represents t k The latitude, longitude and height of the c-th measurement vector of the time sub-node s are different from the latitude, longitude and height of the master node after lever arm compensation;
respectively represents t k Difference values of course angle, pitch angle and roll angle of the c-th quantity vector of the sub-node s at the moment and the course angle, pitch angle and roll angle of the main node;
the steps of the child node s to construct the bootstrap measurement set are as follows:
at t k At the moment, the c-th vector at the child node s
On the basis, L bootstrap measurement vectors are constructed by adding noise disturbance to the measurement vectors
These bootstrap measurement vectors constitute a set of bootstrap measurement sets for the child node s,
wherein the content of the first and second substances,
is shown at t k The c-th vector of the time-of-day child node s
Generating the first bootstrap measurement vector based on the first bootstrap measurement vector;
is at least
Based on the increased disturbance noise when generating the first bootstrap measurement vector,
the measured noise v with the child node s s Have the same statistical properties, i.e.
Is white gaussian noise that satisfies zero mean; v. of s Has a covariance of
As can be seen from the nature of the gaussian distribution,
by the method, NxL bootstrap measurement vectors at the child node s can be generated
Order to
Denotes the child node s at t k The c-th virtual metrology set at time:
to pair
The virtual measurement vectors in (1) are marked uniformly to make
At this time
Rewritable as follows:
in the step (3), the virtual measurement vectors in each virtual measurement set are respectively sampled by using a Markov chain Monte Carlo sampling method to obtain t k The fusion measurement set of each child node at a moment specifically comprises the following steps:
1) computing a confidence probability and an acceptance probability for a set of virtual metrics
According to Metropolis-Hastings sampling principle in Markov chain Monte Carlo sampling method, N virtual measurement sets of subnodes s
In which two sets are arbitrarily selected
Randomly extracting a virtual metrology vector from each of the two sets
And
and calculating corresponding confidence level
And
wherein
The measured variances are respectively
Is twice of the measurement variance, the reliability calculation formula is:
wherein the content of the first and second substances,
is the average of the virtual measurement vectors in the N virtual measurement sets at the child node s,
for measuring noise
Of the covariance matrix
Determinant or measure noise of
Of the covariance matrix
Determinant (c).
According to the credibility
And
calculating out
The acceptance probability between:
the acceptance probabilities are 1 and
the smallest number in;
2) generating a fused measurement set
The selection method of the fusion measurement vector is as follows: let χ be a random number generated from the random distribution U (0,1), when the elements are extracted
And
corresponding acceptance probability
When the random number x is larger than or equal to the predetermined value, the selection is made
As a fusion measurement vector; when probability of acceptance
Less than the random number χ, will
As a fusion measurement vector, x, y is 1,2, …, N, x ≠ y, N x ,n y ＝0,1,…,L；
Repeating the sampling processes 1) and 2) for M times, wherein M is 2L, and obtaining 20 fusion measurement vectors; are respectively marked as Z s (1),Z s (2),…Z s (M), then t k Fusion measurement set theta of time child node s s ＝{Z s (1),Z s (2),…Z s (M)}
In the step (4), the consistency fusion method based on the statistical confidence distance is used for calculating the mutual support degree of any two fusion measurement vectors in each fusion measurement set to obtain each sub-node t k The effective measurement vector and the noise matrix of the moment comprise the following steps:
1) calculating a measurement noise matrix corresponding to the fusion measurement vector in the fusion measurement set
Respectively calculating a fusion measurement set theta by utilizing a consistency fusion method based on statistical confidence distance s And s is 1,2, … N, any two of which fuse with the measurement vector Z s (. alpha.) and Z s Confidence distance of mutual support degree between (beta)
The calculation method is as follows:
wherein, the first and the second end of the pipe are connected with each other,
are respectively a fusion measurement vector Z s (α)、Z s (β) a noise matrix;
from the above expression, it can be seen that:
the larger the value, the larger the difference between the two fused measurement vectors, and Z is at this time s (. alpha.) and Z s The weaker the degree of mutual support between (. beta.); conversely, the stronger the strength;
on the basis of this, so as to
For an element, a consistency matrix D is defined s Comprises the following steps:
then, a consistency matrix D is calculated s Maximum modulus eigenvalue of [ lambda ] - [ lambda ] 1 ,λ 2 ,…λ M ] T And the corresponding feature vector Y ═ Y 1 ,Y 2 ,…Y M ] T Unitizing Y to obtain
Order weight vector
Respectively as a fusion measurement vector Z s (1),Z s (2),…Z s (M) weight, child node s fusion measurement set Θ s Weighted mean of
The calculation method of (2) is as follows:
calculating a fusion measurement set theta according to the basic principle of calculating variance in mathematical statistics s The measurement noise matrix corresponding to each fusion measurement vector
The calculation method is as follows:
2) calculating effective measurement vector and its measurement noise matrix
In a consistency matrix D s Removing diagonal elements from the list and searching for a minimum value of no more than epsilon in the remaining elements
Namely:
wherein epsilon is a judgment threshold value, and epsilon is 0.5; the minimum value will be calculated
2 fusion measurement vectors are respectively marked as Z s (p) and Z s (q); for the fusion measurement vector Z s (p) and Z s (q) performing fusion processing by using maximum likelihood estimation method, and resetting Z using fusion result s (p)；Z s (p) and Z s The fusion method of (q) is:
wherein, the first and the second end of the pipe are connected with each other,
are respectively a fusion measurement vector Z s (p)、Z s (q) the noise matrix;
will Z s (p) and Z s (q) fusion and resetting Z s (p) after deleting the fused measurement set Θ s The q-th fused measurement vector Z s (q), q is a number, and the number is greater than the fusion measurement vector Z of q s (q+1)…Z s The number of (M) is reduced by 1, namely, the number is changed into Z s (q)…Z s (M-1); thus, a fused measurement set theta with the fused measurement vector number reduced by 1 is obtained s ', using a combination of theta s ' reset theta s I.e. theta s ＝Θ s ′；
In the process of fusion
As post fusion Z s (p) the measured noise matrix; repeating the above fusion process of 1) and 2) until the minimum confidence distance in the newly generated consistency matrix
Above a threshold epsilon, the fusion is ended, i.e.
Then the fusion is finished;
at this time, the fused measurement vector participating in the fused reset most times in the final fused measurement set is made to be Z s (h) The fusion measurement vector is the final fusion result; z s (h) I.e. t k The effective measurement vector of the time child node s is recorded as Z' s The measurement noise matrix corresponding to the effective measurement vector is denoted as R' s 。
Establishing a transfer alignment model of each child node in the step (5), specifically comprising the following steps:
1) establishing a sub IMU inertial navigation error equation
For the child node s, s is 1,2, …, N, the inertial navigation error equation is composed of an attitude error equation, a velocity error equation, a position error equation and an inertial instrument error equation.
a) Attitude error equation:
wherein the content of the first and second substances,
the misalignment angle resolved for the strapdown at the child node s,
and
east, north and sky misalignment angles of the child node s under its navigation system, respectively;
the navigation system of the child node s is the projection of the rotation angular speed of the inertial system relative to the navigation system in the navigation system;
is composed of
The calculation error of (2);
calculating an attitude matrix between the carrier system and the relative calculation navigation system for the subnode s obtained by strapdown calculation;
is the projection of the gyro random constant drift of the sub IMU at the sub node s under its carrier system,
and
respectively the components of the gyro on the x axis, the y axis and the z axis of the carrier system in a random constant drift manner;
is the projection of the gyrodrandom white noise of the sub-IMU at the sub-node s under its carrier system,
respectively the components of the gyro random white noise on the x axis, the y axis and the z axis of the carrier system;
b) the velocity error equation:
wherein the content of the first and second substances,
and
are respectively child nodess east, north and sky speed errors resolved by strapdown;
and
east, north and sky speeds respectively solved for the child node s strapdown;
and
respectively measuring components of specific force on an x axis, a y axis and a z axis of a carrier system of the sub-IMU accelerometer at the sub-node s;
for the projection of the sub-IMU accelerometer random constant bias under its carrier system at sub-node s,
and
respectively biasing the components of the accelerometer on the x axis, the y axis and the z axis of the carrier system at random constant values;
and
respectively, sub-IMU accelerometer random white noise at sub-node sThe components of sound in the x-axis, y-axis and z-axis of the carrier system;
for the projection of the global coordinate system on the navigation system of the sub-node s with respect to the rotational angular velocity of the inertial system, ω ie The angular velocity of the earth is approximately equal to 15.04 degrees/h;
projecting the navigation system at the sub-node s on the navigation system of the sub-node s relative to the rotation angular speed of the earth coordinate system;
and
respectively the main curvature radius of a child node s along a meridian circle and a prime unit circle;
c) position error equation:
wherein L is s 、λ s 、H s Respectively resolving the latitude, longitude and altitude of the child node s in a strapdown manner; delta L s 、δλ s 、δH s Respectively solving a latitude error, a longitude error and an altitude error for the sub-node s in a strapdown mode;
and
respectively the main curvature radius of a child node s along a meridian circle and a prime unit circle;
wherein
East and north velocities at the child node s, respectively;
d) inertial instrument error equation:
the calibration compensated inertial instrument error is typically approximated as a random constant and white noise. The random constant can be described by the following differential equation:
wherein the content of the first and second substances,
the gyroscope random constant value drift of the sub IMU at the sub node s in the x axis, the y axis and the z axis of the carrier system of the sub IMU;
randomly biasing the accelerometers of the sub-IMU at the sub-node s in the x-axis, y-axis and z-axis of the carrier system;
2) establishing a child node transfer alignment mathematical model
a) Establishing a system equation
The transmission alignment is carried out by adopting a position and attitude matching mode, and the system state equation of the child node s is as follows:
wherein, X s Is a state variable; f s Is a state transition matrix; g s Is a system noise matrix; w is a group of s The system noise is assumed to be zero mean Gaussian white noise; x s 、F s 、G s And W s The expression of (a) is as follows:
wherein the content of the first and second substances,
wherein the content of the first and second substances,
and
the components of the specific force measured by the sub IMU accelerometer at the sub-node s in the east, north and sky directions of its navigation system, respectively.
b) Establishing a measurement equation
Child node s valid measurement vector Z' s The specific expression of (A) is as follows:
Z′ s ＝[δψ′ s δθ′ s δγ′ s δL′ s δλ′ s δH′ s ] T s＝1,2,…,N
in the formula, delta psi' s 、δθ′ s And δ γ' s Respectively obtaining the difference values of course, pitch, roll and main node course, pitch and roll in the effective measurement vector of the sub-node s; delta L' s 、δλ′ s And δ H' s The difference values of the latitude, longitude and altitude in the effective measurement vector of the sub-node s and the latitude, longitude and altitude of the main node after lever arm compensation are respectively.
The measurement equation for the child node s is:
Z′ s ＝H s X s +v s
in the formula, a measuring matrix
Wherein:
wherein
Is a sub-node s attitude matrix
The element of the row a and the column b, a and b are 1,2 and 3; namely:
in the step (6), transfer alignment based on Kalman filtering is performed to estimate t k The position error, the speed error and the attitude error of each sub-node are carried out at the moment, and the motion parameters of each sub-node are correctedThe method comprises the following specific steps:
1) estimating position, velocity and attitude errors of each child node
Aligning a mathematical model based on the transmission of each child node established in the step (5), and obtaining t in the step (4) k Respectively taking the effective measurement vector of each sub-node at the moment as the measurement vector in Kalman filtering, and estimating the t of each sub-node by utilizing the Kalman filtering k Position error delta L of moment strapdown resolving s 、δλ s 、δH s Error in velocity
And angle of misalignment
Where s is 1,2, … N.
2) Motion parameter correction
Correcting the strapdown resolving result of each sub-node by using the estimation result of the Kalman filtering, wherein the correction comprises position correction, speed correction and attitude correction, and the method comprises the following specific steps:
a) position correction
L s′ ＝L s -δL s ,λ s′ ＝λ s -δλ s ,H s′ ＝H s -δH s ,s＝1,2,...N
Wherein L is s′ 、λ s′ 、H s′ Are each t k The corrected latitude, longitude and altitude at the time child node s;
b) velocity correction
Wherein the content of the first and second substances,
are each t k The east speed, the north speed and the sky speed after correction at the time subnode s;
c) attitude correction
Calculating t k Navigation system n at time child node s s And calculating navigation system n' s Inter-conversion matrix
The updated attitude matrix at the child node s is:
matrix of gestures
Is recorded as:
wherein, T ab As a matrix of postures
The elements of the row a and the column b, a and b are 1,2 and 3; the main values of the course angle, the pitch angle and the roll angle after the s-th sub-node correction
Respectively as follows:
the value ranges of the course angle, the pitch angle and the roll angle are respectively defined as [0,2 pi ]]、
[-π,+π](ii) a The true values of the course angle, pitch angle and roll angle are determined by the following equationsDetermining:
by for each child node t k Correcting the speed, position and attitude of the moment to obtain the speed, position and attitude information of the child node with higher precision, and finishing t k The transfers of all child nodes are aligned at time.
Compared with the prior art, the invention has the advantages that:
the method combines a measurement bootstrap strategy and a consistency judgment method based on statistical confidence distance around the aim of improving the overall accuracy of airborne DPOS multi-node motion parameters, and performs data fusion on two fusion measurement vectors with high mutual support degree in a fusion measurement set by using a consistency fusion method based on statistical confidence distance on the basis of motion parameter information of all sub-nodes, so that the influence of the fusion measurement vectors with low mutual support degree on the accuracy of effective measurement vectors is overcome, more accurate effective measurement vectors are obtained, and the overall estimation accuracy of the motion parameters of the sub-nodes is improved.
Drawings
FIG. 1 is a flow chart of the present invention.
Detailed Description
As shown in FIG. 1, the specific method of the present invention is implemented as follows:
1. resolving t k The method comprises the following steps of obtaining the positions, speeds and postures of a main node and each sub-node at a moment, and obtaining redundant positions and postures of each sub-node:
(1) definition of coordinate system
1) Earth's center inertial coordinate system, i system and inertial system for short
The origin of the coordinate system is geocentric, x i And y i Axis in the equatorial plane of the earth, z i The axis points to the spring equinox point, the axis points to the earth polar axis, and y is determined by the right hand rule i An axial direction;
2) terrestrial coordinate system, e system for short
The global coordinate system is a seat fixedly connected on the earthThe coordinate system, which rotates with the earth, is approximately regarded as the rotation angle rate omega of the earth relative to the inertial coordinate system ie Rotation, omega ie The angle is approximately equal to 15.04 degrees/h; the origin of the coordinate system is the center of the earth, z e Axis pointing towards earth polar axis, x e Axis through zero meridian, y being determined by the right hand rule e An axial direction;
3) carrier coordinate system, carrier system for short
Its origin of coordinates is the center of mass of the carrier, x b The axis pointing to the right along the transverse axis of the carrier, y b The axis pointing forwards along the longitudinal axis of the carrier, z b The axis points upward along the vertical axis of the carrier; for main and sub nodes in DPOS, b s The carrier systems respectively represent a main node and an s-th sub-node, wherein s is 1,2, …, N and N are the number of the sub-nodes;
4) navigation coordinate system, navigation system for short
The navigation system is a coordinate system selected according to work requirements when navigation parameters are solved, the navigation system is taken as a northeast geographical coordinate system, the navigation system of the main node is represented by n, and the navigation system and the calculation navigation system of the s-th sub-node are respectively represented by n s And n' s Representing;
(2) the main node and each sub-node carry out strapdown resolving
The main node and all the sub-nodes are solved through strapdown to respectively obtain the main node and all the sub-nodes t k Position of time [ L m λ m H m ] T And [ L s λ s H s ] T And attitude [ psi m θ m γ m ] T And [ psi s θ s γ s ] T Speed, velocity
And
wherein L is m 、λ m And H m Respectively representing the latitude, longitude and altitude of the master node, L s 、λ s And H s Respectively representing the latitude, longitude and altitude of the s-th child node; psi m 、θ m And gamma m Respectively representing the heading, pitch and roll angles of the master node,. psi s 、θ s And gamma s Respectively representing a course angle, a pitch angle and a roll angle of the s-th sub-node;
and
respectively representing the east-direction speed, the north-direction speed and the sky-direction speed of the main node,
and
respectively representing the east speed, the north speed and the sky speed of the s-th child node;
(3) acquisition of redundant position and posture of each child node
The redundant position and posture of a certain sub-node can be calculated by the positions and postures of the other sub-nodes and the corresponding relative positions and relative angles by utilizing the relative positions and relative angles between the sub-nodes measured by the fiber bragg grating sensors arranged on the wings; that is, any child node can obtain N-1 sets of redundant position and attitude information.
1) Acquisition of redundant positions of child nodes
T measured by fiber grating sensor k At any moment, two sub-nodes s and s * Relative position of
Wherein
Respectively representing two child nodes s, s * Difference in latitude, difference in longitude, difference in altitude, s * ＝1,2,…,N,s≠s * ；
t k At the moment, the position obtained after strapdown resolving at the position of the child node s is [ L ] s λ s H s ] T And s is 1,2, …, N, and the positions of the rest N-1 sub-nodes after strapdown resolving are expressed as
The N-1 sets of redundant location information at the child node s can be represented as:
by N-1 sets of redundant locations at the child node s
And the position [ L ] of the byte point obtained by strapdown solution s λ s H s ] T Then t at the child node s is obtained k N sets of positions of time, note
2) Acquisition of child node redundancy posture
T measured by fiber grating sensor k Any two sub-nodes s, s at any moment * ，s、s * ＝1,2,…,N,s≠s * Relative angle of rotation of
Wherein
Respectively representing child nodes s * The carrier system of (b) and the carrier system of the child node s have relative rotation angles among the x axis, the y axis and the z axis; by passing
Can calculate the child nodes s, s * Relative attitude matrix between carrier systems
The calculation method is as follows:
the attitude matrix between the carrier system and the computational navigation system obtained by strapdown calculation at the child node s is
Other N-1 sub-nodes are subjected to strapdown solution to calculate respective attitude matrix of
N-1 redundant attitude matrices at child node s
Can be expressed as:
wherein the content of the first and second substances,
computing a navigation coordinate system for a child node s to the child node s * And calculating a transformation matrix between the navigation systems by the following method:
wherein the content of the first and second substances,
are child nodes s, respectively * And a transformation matrix between the earth coordinate system and each computed navigation system; l is s 、
And λ s 、
Respective sub-nodes s, s * Latitude and longitude of;
t k time of day, by the attitude matrix at the child node s
And N-1 redundant attitude matrices
N groups of attitude angles including a course angle, a pitch angle and a roll angle can be obtained through calculation; the calculation method is as follows:
matrix of gestures
Is recorded as:
wherein the content of the first and second substances,
as a matrix of gestures
The elements in the row a and the column b, a and b are 1,2 and 3; the heading angle psi at the child node s s Angle of pitch theta s And roll angle γ s Principal value of
Respectively as follows:
the value ranges of the course angle, the pitch angle and the roll angle are respectively defined as [0,2 pi ]]、
[-π,+π](ii) a Then the heading angle psi s Angle of pitch theta s And roll angle γ s Are respectively determined by the following formula:
according to the calculation method of the attitude angle, N-1 redundant attitude matrixes at the position of the child node s can be calculated
Corresponding N-1 groups of attitude angle information; thereby, unite and consist of
1 group of attitude angle information is calculated, and t at the position of the child node s is obtained k N sets of attitude information of time, and is recorded as
2. Generating t using a measurement bootstrapping strategy k The virtual measurement set of each child node at a moment specifically comprises the following steps:
at t k At any time, for any child node s, s is 1,2, …, N measured vector quantities of the child node can be obtained
The expression is as follows:
wherein the content of the first and second substances,
respectively represents t k The latitude, longitude and altitude of the c-th measurement vector of the time sub-node s are different from the latitude, longitude and altitude of the main node after lever arm compensation;
respectively represents t k Difference values of course angle, pitch angle and roll angle of the c-th quantity vector of the sub-node s at the moment and the course angle, pitch angle and roll angle of the main node;
the steps of the child node s to construct the bootstrap measurement set are as follows:
at t k Time of day, c-th quantity at child node s
On the basis, L bootstrap measurement vectors are constructed by adding noise disturbance to the measurement vectors
These bootstrap measurement vectors constitute a set of bootstrap measurement sets for the child node s,
wherein the content of the first and second substances,
is shown int k The c-th vector of the time subnode s
Generating the first bootstrap measurement vector based on the first bootstrap measurement vector;
is at the same time
Based on the added disturbance noise when generating the first bootstrap measurement vector,
the measured noise v with the child node s s Having the same statistical properties, i.e.
Is white gaussian noise that satisfies zero mean; v. of s Has a covariance of
As can be seen from the nature of the gaussian distribution,
by the method, NxL bootstrap measurement vectors at the child node s can be generated
Order to
Denotes the child node s at t k The c-th virtual measurement set at time:
to is in pair with
The virtual measurement vector in (1) is marked uniformly, so that
At this time
Rewritable as follows:
3. respectively sampling the virtual measurement vectors in each virtual measurement set by using a Markov chain Monte Carlo sampling method to obtain t k The fusion measurement set of each child node at a moment specifically comprises the following steps:
(1) computing a confidence probability and an acceptance probability for a set of virtual metrics
According to Metropolis-Hastings sampling principle in Markov chain Monte Carlo sampling method, N virtual measurement sets of child nodes s
In which two sets are arbitrarily selected
Randomly extracting a virtual metrology vector from each of the two sets
And calculating corresponding credibility
And
wherein
The measured variances are respectively
Is twice of the measurement variance, the reliability calculation formula is:
wherein the content of the first and second substances,
is the average of the virtual measurement vectors in the N virtual measurement sets at the child node s,
for measuring noise
Covariance matrix of
Determinant or measure noise of
Covariance matrix of
Determinant (c).
According to the credibility
And
computing
The acceptance probability between:
the acceptance probabilities are 1 and
the smallest number in;
(2) generating a fused measurement set
The selection method of the fusion measurement vector is as follows: let χ be a random number generated from the random distribution U (0,1), when the elements are extracted
And
corresponding acceptance probability
When the random number x is larger than or equal to the predetermined value, the selection is made
As a fusion measurement vector; when probability of acceptance
Less than the random number χ, will
As a fusion measurement vector, x, y is 1,2, …, N, x ≠ y, N x ,n y ＝0,1,…,L；
Repeating the sampling processes (1) and (2) for M times, wherein M is 2L, and obtaining 20 fusion measurement vectors; are respectively marked as Z s (1),Z s (2),…Z s (M), then t k Fusion measurement set theta of time child node s s ＝{Z s (1),Z s (2),…Z s (M)}。
4. Using statistical-based confidence distancesThe consistency fusion method of (1) calculates the mutual support degree of any two fusion measurement vectors in each fusion measurement set to obtain each sub-node t k The effective measurement vector and the noise matrix of the moment comprise the following steps:
(1) calculating a measurement noise matrix corresponding to the fusion measurement vector in the fusion measurement set
Respectively calculating a fusion measurement set theta by utilizing a consistency fusion method based on statistical confidence distance s And s is 1,2, … N, any two of which fuse with the measurement vector Z s (. alpha.) and Z s Confidence distance of mutual support degree between (beta)
The calculation method is as follows:
wherein, the first and the second end of the pipe are connected with each other,
are respectively a fusion measurement vector Z s (α)、Z s (β) a noise matrix;
from the above expression, it can be seen that:
the larger the value, the larger the difference between the two fused measurement vectors, and Z is at this time s (. alpha.) and Z s The weaker the degree of mutual support between (. beta.); conversely, the stronger the strength; on the basis of this, so as to
For an element, a consistency matrix D is defined s Comprises the following steps:
then, a consistency matrix D is calculated s Maximum mode eigenvalue of [ lambda ] - [ lambda ] 1 ,λ 2 ,…λ M ] T And the corresponding feature vector Y ═ Y 1 ,Y 2 ,…Y M ] T Unitizing Y to obtain
Order weight vector
Are respectively a fusion measurement vector Z s (1),Z s (2),…Z s (M) weight, child node s fusion measurement set Θ s Weighted mean value Z of s The calculation method of (2) is as follows:
calculating a fusion measurement set theta according to the basic principle of calculating variance in mathematical statistics s The measurement noise matrix corresponding to each fusion measurement vector
The calculation method is as follows:
(2) calculating effective measurement vector and its measurement noise matrix
In a consistency matrix D s Removing diagonal elements from the list and searching for a minimum value of no more than epsilon in the remaining elements
Namely:
wherein epsilon is a judgment threshold value, and epsilon is 0.5; the minimum value will be calculated
Respectively recording the 2 fused measurement vectors as Z s (p) and Z s (q); for the fusion measurement vector Z s (p) and Z s (q) performing fusion processing by using maximum likelihood estimation method, and resetting Z by using fusion result s (p)；Z s (p) and Z s The fusion method of (q) is:
wherein the content of the first and second substances,
are respectively a fusion measurement vector Z s (p)、Z s (q) the noise matrix;
will Z s (p) and Z s (q) fusion and resetting Z s (p) after deleting the fused metrology set Θ s The q-th fused measurement vector Z s (q), q is a number, and the number is greater than the fused measurement vector Z of q s (q+1)…Z s The number of (M) is reduced by 1, namely, the number is changed into Z s (q)…Z s (M-1); thus, a fused measurement set theta with the fused measurement vector number reduced by 1 is obtained s ', using a combination of theta s ' reset theta s I.e. theta s ＝Θ s ′；
In the process of fusion
As post fusion Z s (p) the measured noise matrix; repeat the above(1) The fusion process of (1) and (2) until the minimum confidence distance in the newly generated consistency matrix
Above a threshold epsilon, the fusion is ended, i.e.
The fusion is ended.
At this time, the fused measurement vector participating in the fused reset most times in the final fused measurement set is made to be Z s (h) The fusion measurement vector is the final fusion result; will Z s (h) As t k The effective measurement vector of the time child node s is recorded as Z' s The measured noise matrix corresponding to the valid measured vector is denoted as R' s 。
5. Establishing a transfer alignment model of each child node, which comprises the following specific steps:
for the child node s, s is 1,2, …, N, the inertial navigation error equation is composed of an attitude error equation, a velocity error equation, a position error equation, and an inertial instrument error equation.
(1) Establishing sub IMU inertial navigation error model
1) Attitude error equation:
wherein the content of the first and second substances,
the misalignment angle resolved for the strapdown at the child node s,
and
respectively representing east, north and sky misalignment angles of the child node s in a navigation coordinate system;
the navigation system of the child node s is the projection of the rotation angular speed of the inertial system relative to the navigation system in the navigation system;
is composed of
The calculation error of (2);
calculating an attitude matrix between a carrier coordinate system and a relative calculation navigation system for the subnode s obtained by strapdown calculation;
is the projection of the gyro random constant drift of the sub IMU at the sub node s under its carrier system,
and
respectively representing components of the gyro on an x axis, a y axis and a z axis of the carrier system in a random constant drift manner;
is the projection of the gyromagnetic white noise of the sub-IMU at the sub-node s under its carrier system,
components of the gyro random white noise on an x axis, a y axis and a z axis of the carrier system are respectively;
2) the velocity error equation:
wherein the content of the first and second substances,
and
east, north and sky speed errors respectively solved for the child node s strap-down;
and
east, north and sky speeds respectively solved for the child node s strapdown;
and
respectively measuring the components of the specific force of the sub IMU accelerometer at the sub node s on the x axis, the y axis and the z axis of a carrier system of the sub IMU accelerometer;
for the projection of the sub-IMU accelerometer random constant bias under its carrier system at sub-node s,
and
respectively randomly and constantly biasing components of the accelerometer on an x axis, a y axis and a z axis of the carrier system;
and
respectively the components of the random white noise of the sub IMU accelerometer at the sub node s on the x axis, the y axis and the z axis of the carrier system;
for the projection of the global coordinate system on the navigation system of the sub-node s with respect to the rotational angular velocity of the inertial system, ω ie The rotation angular speed of the earth is about 15.04 degrees/h;
projecting the navigation system at the position of the child node s on the navigation system of the child node s relative to the rotation angular speed of the earth coordinate system;
and
respectively the main curvature radius of a child node s along a meridian circle and a prime unit circle;
3) position error equation:
wherein L is s 、λ s 、H s Respectively resolving the latitude, longitude and altitude of the child node s by strapdown; delta L s 、δλ s 、δH s Respectively solving a latitude error, a longitude error and an altitude error for the sub-node s in a strapdown mode;
and
respectively the main curvature radius of a child node s along a meridian circle and a prime unit circle;
wherein
East and north velocities at the child node s, respectively;
4) inertial instrument error equation:
the error of the inertia instrument after calibration compensation is generally approximate to a random constant value and random white noise; the random constant can be described by the following differential equation:
wherein the content of the first and second substances,
the gyroscope random constant drift of the sub IMU at the sub node s in the x axis, the y axis and the z axis of the carrier coordinate system of the sub IMU;
randomly biasing the accelerometers of the sub-IMU at the sub-node s in the x-axis, y-axis and z-axis of the carrier system;
(2) establishing a child node transfer alignment mathematical model
1) Establishing a system equation
The transmission alignment is carried out by adopting a matching mode of 'position + attitude', and the system state equation of the child node s is as follows:
wherein X s Is a state variable; f s Is a state transition matrix; g s Is a system noise matrix; w s Is systematic noise and is assumed to be zero mean Gaussian whiteNoise; x s 、F s 、G s And W s The expression of (a) is as follows:
wherein the content of the first and second substances,
and
the components of the specific force measured by the sub-IMU accelerometer at the sub-node s in its navigational frame east, north and sky, respectively.
2) Establishing a measurement equation
Child node s valid measurement vector Z' s The specific expression of (A) is as follows:
Z′ s ＝[δψ′ s δθ′ s δγ′ s δL′ s δλ′ s δH′ s ] T s＝1,2,…,N (41)
in the formula, delta psi' s 、δθ′ s And δ γ' s Respectively obtaining the difference values of course, pitch, roll and main node course, pitch and roll in the effective measurement vector of the sub-node s; delta L' s 、δλ′ s And δ H' s The difference values of the latitude, longitude and altitude in the effective measurement vector of the sub-node s and the latitude, longitude and altitude of the main node after lever arm compensation are respectively.
The measurement equation for the child node s is:
Z′ s ＝H s X s +v s (42)
in the formula (I), the compound is shown in the specification,
wherein the content of the first and second substances,
attitude matrix being a child node s
The elements of the row a and the column b, a and b are 1,2 and 3; namely:
6. transfer alignment based on Kalman filtering is carried out to estimate t k The method comprises the following steps of correcting the motion parameters of each sub-node at any moment by using the position error, the speed error and the attitude error of each sub-node, and specifically comprises the following steps:
(1) estimating position, velocity and attitude errors of each child node
Based on the transfer alignment mathematical model of each child node established in step 5, and obtaining t in step 3 k Respectively taking the effective measurement vector of each sub-node at the moment as the measurement vector in Kalman filtering, and estimating the t of each sub-node by utilizing the Kalman filtering k Position error delta L of moment strapdown resolving s 、δλ s 、δH s Error in velocity
And angle of misalignment
Where s is 1,2, … N.
(2) Motion parameter correction
Correcting the strapdown resolving result of the s sub-nodes by using the result of the cubature Kalman filtering, wherein the result comprises the following specific steps:
correcting strapdown resolving results of the sub-nodes by using the estimation result of the Kalman filtering, wherein the strapdown resolving results comprise position correction, speed correction and attitude correction, and the method comprises the following specific steps:
1) position correction
Wherein L is s′ 、λ s′ 、H s′ Are each t k The corrected latitude, longitude and altitude at the time child node s;
2) velocity correction
Wherein the content of the first and second substances,
are each t k The east speed, the north speed and the sky speed after correction at the time subnode s;
3) attitude correction
Calculating t k Navigation system n at time child node s s And calculating navigation system n' s Inter-conversion matrix
The updated attitude matrix at the child node s is:
matrix of gestures
Is recorded as:
wherein, T ab As a matrix of gestures
The element in the row a and the column b, a is 1,2, 3, b is 1,2, 3; the main values of the course angle, the pitch angle and the roll angle after the s-th sub-node correction
Respectively as follows:
the value ranges of the course angle, the pitch angle and the roll angle are respectively defined as [0,2 pi ]]、
[-π,+π](ii) a The true values of the heading angle, pitch angle and roll angle are then determined by:
by for each child node t k Correcting the speed, position and attitude of the moment to obtain the speed, position and attitude information of the child node with higher precision, and finishing t k The transfers of all child nodes are aligned at time.
Those skilled in the art will appreciate that the invention may be practiced without these specific details.
Claims (7)
1. A method for transfer alignment of airborne DPOS based on statistical confidence distance measurement bootstrapping comprises the following specific steps:
1.1 solving for t k The positions, speeds and postures of the main node and each sub-node at the moment, and the redundant positions and postures of each sub-node;
1.2 Using the measurement bootstrapping strategy to generate t k A virtual measurement set of each child node at a moment;
1.3 sampling the virtual measurement vectors in each virtual measurement set respectively by using Markov chain Monte Carlo sampling method to obtain t k The fusion measurement set of each child node at the moment;
1.4 calculating the mutual support degree of any two fusion measurement vectors in each fusion measurement set by using a consistency fusion method based on statistical confidence distance to obtain each sub-node t k The effective measurement vector and its noise matrix of the moment;
1.5 establishing a transfer alignment model of each child node;
1.6 transfer alignment based on Kalman filtering is carried out to estimate t k The position error, the speed error and the attitude error of each sub-node are obtained at the moment, and the motion parameters of each sub-node are corrected;
1.7t k ＝t k + 1; repeating steps 1.1 to 1.7 until all child nodes are completedThe transfer of time instants is aligned.
2. The method of claim 1, wherein the onboard DPOS transfer alignment method based on statistical confidence distance measurement bootstrapping comprises: in the step 1.1, t is solved k The method comprises the following steps of obtaining the positions, speeds and postures of a main node and each sub-node at a moment, and obtaining redundant positions and postures of each sub-node:
2.1 coordinate System definition
(1) Earth's center inertial coordinate system, i system and inertial system for short
The origin of the coordinate system is geocentric, x i And y i Axis in the equatorial plane of the earth, z i The axis points to the spring equinox point, the axis points to the earth polar axis, and y is determined by the right hand rule i An axial direction;
(2) terrestrial coordinate system, e system for short
The earth coordinate system is a coordinate system which is fixedly connected with the earth and rotates along with the earth, and approximately considers that the earth coordinate system rotates at the earth rotation angular rate omega relative to the inertial coordinate system ie Rotation, omega ie The angle is approximately equal to 15.04 degrees/h; the origin of the coordinate system is the center of the earth, z e Axis directed to earth polar axis, x e Axis through zero meridian, y being determined by the right hand rule e An axial direction;
(3) carrier coordinate system, carrier system for short
Its origin of coordinates is the center of mass of the carrier, x b The axis pointing to the right along the transverse axis of the carrier, y b The axis pointing forwards along the longitudinal axis of the carrier, z b The axis points upwards along the vertical axis of the carrier; for the main and sub-nodes, b, in DPOS s The carrier systems respectively represent a main node and an s-th sub-node, wherein s is 1,2, …, N and N are the number of the sub-nodes;
(4) navigation coordinate system, navigation system for short
The navigation system is a coordinate system selected according to work requirements when navigation parameters are solved, the navigation system is taken as a northeast geographical coordinate system, the navigation system of the main node is represented by n, and the navigation system and the calculation navigation system of the s-th sub-node are respectively represented by n s And n s ' represents;
2.2 strapdown resolving between the Main node and the sub-nodes
The main node and all the sub-nodes are solved through strapdown to respectively obtain the main node and all the sub-nodes t k Position of time [ L m λ m H m ] T And [ L s λ s H s ] T And attitude [ psi m θ m γ m ] T And [ psi s θ s γ s ] T Speed, velocity
And
wherein L is m 、λ m And H m Respectively representing the latitude, longitude and altitude of the master node, L s 、λ s And H s Respectively representing the latitude, longitude and altitude of the s-th sub-node; psi m 、θ m And gamma m Respectively representing the heading, pitch and roll angles of the master node,. psi s 、θ s And gamma s Respectively representing a course angle, a pitch angle and a roll angle of the s-th sub-node;
and
respectively representing the east-direction speed, the north-direction speed and the sky-direction speed of the main node,
and
respectively representing the east speed, the north speed and the sky speed of the s-th child node;
2.3 obtaining redundant position and attitude of each child node
The redundant position and posture of a certain sub-node can be calculated by the positions and postures of the other sub-nodes and the corresponding relative positions and relative angles by utilizing the relative positions and relative angles between the sub-nodes measured by the fiber bragg grating sensors arranged on the wings; that is, any child node can obtain N-1 sets of redundant position and attitude information.
(1) Acquisition of redundant positions of child nodes
T measured by fiber grating sensor k Any two sub-nodes s, s at any moment * Relative position of
Wherein
Respectively represent two child nodes s, s * Difference in latitude, difference in longitude, difference in altitude, s * ＝1,2,…,N,s≠s * ；
t k At the moment, the position obtained after the sub-node s is subjected to strapdown calculation is [ L ] s λ s H s ] T And the position after the strapdown calculation of the rest N-1 sub-nodes is expressed as s which is 1,2, … and N
s * ＝1,2,…,N,s * Not equal to s; the N-1 sets of redundant location information at the child node s can be expressed as:
by N-1 sets of redundant locations at the child node s
And the position [ L ] of the byte point obtained by strapdown solution s λ s H s ] T And thenObtain t at the child node s k N sets of positions of time, note
(2) Acquisition of child node redundancy posture
T measured by fiber grating sensor k Any two sub-nodes s, s at any moment * ，s≠s * Relative angle of rotation of
Wherein
Respectively representing child nodes s * The carrier system of (a) and the carrier system of the child node s have relative rotation angles between the x-axis, the y-axis and the z-axis; by passing
Can calculate the child nodes s, s * Relative attitude matrix between carrier systems
The calculation method is as follows:
the attitude matrix between the carrier system and the computational navigation system obtained by strapdown calculation at the child node s is
The respective attitude matrix calculated by other N-1 child nodes through strapdown solution is
N-1 redundant attitude matrices at child node s
Can be expressed as:
wherein the content of the first and second substances,
computing navigation system for child node s to child node s * And calculating a transformation matrix between the navigation systems by the following method:
wherein the content of the first and second substances,
are child nodes s, respectively * And a transformation matrix between the earth coordinate system and each computed navigation system; l is s 、
And λ s 、
Respective sub-nodes s, s * Latitude and longitude of;
t k time of day, by the attitude matrix at the child node s
And N-1 redundant attitude matrices
N groups of attitude angles including a course angle, a pitch angle and a roll angle can be obtained through calculation; the calculation method is as follows:
matrix of gestures
Is recorded as:
wherein the content of the first and second substances,
as a matrix of postures
The elements of the row a and the column b, a and b are 1,2 and 3; the heading angle psi at the child node s s Angle of pitch theta s And roll angle γ s Principal value of
Respectively as follows:
the value ranges of the course angle, the pitch angle and the roll angle are respectively defined as
Then the heading angle psi s Angle of pitch theta s And roll angle γ s Are respectively determined by the following formula:
according to the calculation method of the attitude angle, N-1 redundant attitude matrixes at the position of the child node s can be calculated
Correspond toN-1 sets of attitude angle information; thereby, unite and consist of
The calculated 1 group of attitude angle information obtains t at the position of the child node s k N sets of attitude information of time, noted as
c＝1,2,…,N。
3. The method of claim 2, wherein the onboard DPOS transfer alignment method based on statistical confidence distance measurement bootstrapping comprises: in the step 1.2, t is generated by using a measurement bootstrap strategy k The virtual measurement set of each child node at a moment comprises the following specific steps:
at t k At any time, for any child node s, s is 1,2, …, N measured vector quantities of the child node can be obtained
The expression is as follows:
wherein the content of the first and second substances,
respectively represents t k The latitude, longitude and altitude of the c-th measurement vector of the time sub-node s are different from the latitude, longitude and altitude of the main node after lever arm compensation;
respectively represents t k Difference values of course angle, pitch angle and roll angle of the c-th quantity vector of the sub-node s at the moment and the course angle, pitch angle and roll angle of the main node;
the steps of the child node s to construct the bootstrap measurement set are as follows:
at t k Time of day, c-th quantity at child node s
On the basis, L bootstrap measurement vectors are constructed by adding noise disturbance to the measurement vectors
L1, 2, … L, L10, these bootstrap measurement vectors constitute a set of bootstrap measurement sets for the child node s,
wherein the content of the first and second substances,
is shown at t k The c-th vector of the time-of-day child node s
Generating the first bootstrap measurement vector based on the first bootstrap measurement vector;
is at least
Based on the added disturbance noise when generating the first bootstrap measurement vector,
the measured noise v with the child node s s Having the same statistical properties, i.e.
Is white gaussian noise that satisfies zero mean; v. of s Has a covariance of
As can be seen from the nature of the gaussian distribution,
by the method, NxL bootstrap measurement vectors at the child node s can be generated
c 1,2, 1, L, N, L; order to
Denotes the child node s at t k The c-th virtual metrology set at time:
to pair
The virtual measurement vector in (1) is marked uniformly, so that
At this time
The rewritables are:
4. the method of claim 3, wherein the onboard DPOS transfer alignment method based on statistical confidence distance measurement bootstrapping comprises: in step 1.3, a Markov chain Monte Carlo sampling method is used to respectively perform virtual measurement setsThe virtual measurement vector in (1) is sampled to obtain t k The fusion measurement set of each child node at a moment specifically comprises the following steps:
4.1 calculating the confidence probability and acceptance probability of the virtual metrology set
According to Metropolis-Hastings sampling principle in Markov chain Monte Carlo sampling method, N virtual measurement sets of child nodes s
c is 1,2, …, N, two sets are arbitrarily selected
Randomly extracting a virtual metrology vector from each of the two sets
And
n x ,n y equal to 0,1, …, L, and calculate the corresponding confidence level
And
wherein
n x ,n y 1, …, the measured variance of L is
Is twice of the measurement variance, the reliability calculation formula is:
wherein the content of the first and second substances,
is the average of the virtual measurement vectors in the N virtual measurement sets at the child node s,
for measuring noise
Covariance matrix of
Determinant or measure noise of
Covariance matrix of
Determinant (c).
According to the degree of confidence
And
computing
The acceptance probability between:
the acceptance probabilities are 1 and
the smallest number in;
4.2 generating a fused metrology set
The selection method of the fusion measurement vector is as follows: let χ be a random number generated from the random distribution U (0,1), when the elements are extracted
And
corresponding acceptance probability
When the random number x is larger than or equal to the predetermined value, the selection is made
As a fusion measurement vector; when probability of acceptance
Less than the random number χ, will
As a fusion measurement vector, x, y is 1,2, …, N, x ≠ y, N x ,n y ＝0,1,…,L；
Repeating the sampling process of 4.1 and 4.2M times, wherein M is 2L, and obtaining 20 fusion measurement vectors; are respectively marked as Z s (1),Z s (2),…Z s (M), then t k Fusion measurement set theta of time child node s s ＝{Z s (1),Z s (2),…Z s (M)}。
5. The method of claim 4, wherein the onboard DPOS transfer alignment method based on statistical confidence distance measurement bootstrapping comprises: in the step 1.4, the consistency fusion method based on the statistical confidence distance is used for calculating the mutual support degree of any two fusion measurement vectors in each fusion measurement set to obtain each sub-node t k Effective measurement vector of time and its noise matrixThe method comprises the following steps:
5.1 calculating the measurement noise matrix corresponding to the fusion measurement vector in the fusion measurement set
Respectively calculating a fusion measurement set theta by utilizing a consistency fusion method based on statistical confidence distance s Any two of the fusion measurement vectors Z of 1,2 and … N s (. alpha.) and Z s Confidence distance of mutual support degree between (beta)
The calculation method is as follows:
wherein the content of the first and second substances,
are respectively a fusion measurement vector Z s (α)、Z s (β) a noise matrix;
from the above expression, it can be seen that:
the larger the value, the larger the difference between the two fused measurement vectors, and Z is at this time s (. alpha.) and Z s The weaker the degree of mutual support between (. beta.); conversely, the stronger the strength;
on the basis of this, so as to
For an element, a consistency matrix D is defined s Comprises the following steps:
then, a consistency matrix D is calculated s Maximum modulus eigenvalue of [ lambda ] - [ lambda ] 1 ,λ 2 ,…λ M ] T And the corresponding feature vector Y ═ Y 1 ,Y 2 ,…Y M ] T Unitizing Y to obtain
Order weight vector
Are respectively a fusion measurement vector Z s (1),Z s (2),…Z s (M) weight, child node s fusion measurement set Θ s Weighted mean of
The calculation method of (2) is as follows:
calculating a fusion measurement set theta according to the basic principle of calculating variance in mathematical statistics s The measurement noise matrix corresponding to each fusion measurement vector
The calculation method is as follows:
5.2 calculating the effective measurement vector and the measurement noise matrix
In a consistency matrix D s Removing diagonal elements from the list and searching for a minimum value of no more than epsilon in the remaining elements
Namely:
wherein epsilon is a judgment threshold value, and epsilon is 0.5; the minimum value will be calculated
Respectively recording the 2 fused measurement vectors as Z s (p) and Z s (q); for the fusion measurement vector Z s (p) and Z s (q) performing fusion processing by using maximum likelihood estimation method, and resetting Z by using fusion result s (p)；Z s (p) and Z s The fusion method of (q) is:
wherein the content of the first and second substances,
are respectively a fusion measurement vector Z s (p)、Z s (q) the noise matrix;
will Z s (p) and Z s (q) fusion and resetting Z s (p) after deleting the fused measurement set Θ s The q-th fused measurement vector Z s (q), q is a number, and the number is greater than the fused measurement vector Z of q s (q+1)…Z s The number of (M) is reduced by 1, namely, the number is changed into Z s (q)…Z s (M-1); thus, a fused measurement set theta with the fused measurement vector number reduced by 1 is obtained s ', using a combination of theta s ' reset theta s I.e. theta s ＝Θ s ′；
In the process of fusion
As fused post-position Z s (p) the measured noise matrix; the above fusion process of 5.1 and 5.2 was repeated until consistency in new generationMinimum confidence distance in matrix
Above a threshold epsilon, the fusion is ended, i.e.
Then the fusion is finished;
at this time, the fused measurement vector participating in the fused reset most times in the final fused measurement set is made to be Z s (h) The fusion measurement vector is the final fusion result; will Z s (h) As t k The effective measurement vector of the time subnode s is marked as Z s ', the measurement noise matrix corresponding to the effective measurement vector is marked as R s ′。
6. The method of claim 1, wherein the onboard DPOS transfer alignment method based on statistical confidence distance measurement bootstrapping comprises: in the step 1.5, the transfer alignment model of each child node is established, and the specific steps are as follows:
6.1 establishing a sub-IMU inertial navigation error equation
For the child node s, s is 1,2, …, N, the inertial navigation error equation is composed of an attitude error equation, a velocity error equation, a position error equation, and an inertial instrument error equation.
(1) Attitude error equation:
wherein the content of the first and second substances,
the misalignment angle resolved for the strapdown at the child node s,
and
respectively representing east, north and sky misalignment angles of the child node s in a navigation coordinate system;
the projection of the navigation system of the sub node s relative to the rotation angular speed of the inertial system in the navigation system is shown;
is composed of
The calculation error of (2);
calculating an attitude matrix between a carrier coordinate system and a relative calculation navigation system for the subnode s obtained by strapdown calculation;
is the projection of the gyro random constant drift of the sub IMU at the sub node s under its carrier system,
and
respectively representing components of the gyro on an x axis, a y axis and a z axis of the carrier system in a random constant drift manner;
is the projection of the gyromagnetic white noise of the sub-IMU at the sub-node s under its carrier system,
components of the gyro random white noise on an x axis, a y axis and a z axis of the carrier system are respectively;
(2) the velocity error equation:
wherein the content of the first and second substances,
and
east, north and sky speed errors respectively solved for the child node s strap-down;
and
east, north and sky speeds respectively solved for the child node s strapdown;
and
respectively measuring the components of the specific force of the sub IMU accelerometer at the sub node s on the x axis, the y axis and the z axis of a carrier system of the sub IMU accelerometer;
for the projection of the sub-IMU accelerometer random constant bias under its carrier system at sub-node s,
and
respectively biasing the components of the accelerometer on the x axis, the y axis and the z axis of the carrier system at random constant values;
and
respectively the components of the random white noise of the sub IMU accelerometer at the sub node s on the x axis, the y axis and the z axis of the carrier system;
for the projection of the global coordinate system on the navigation system of the sub-node s with respect to the rotational angular velocity of the inertial system, ω ie The angular velocity of the earth is approximately equal to 15.04 degrees/h;
projecting the navigation system at the sub-node s on the navigation system of the sub-node s relative to the rotation angular speed of the earth coordinate system;
and
respectively the main curvature radius of a child node s along a meridian circle and a prime unit circle;
(3) position error equation:
wherein L is s 、λ s 、H s Respectively resolving the latitude, longitude and altitude of the child node s in a strapdown manner; delta L s 、δλ s 、δH s Respectively solving a latitude error, a longitude error and an altitude error for the sub-node s in a strapdown mode;
and
respectively the main curvature radius of the child node s along the meridian and the prime circle;
wherein
East and north velocities at the child node s, respectively;
(4) inertial instrument error equation:
the error of the inertia instrument after calibration compensation is generally approximate to a random constant value and random white noise; the random constant can be described by the following differential equation:
wherein, the first and the second end of the pipe are connected with each other,
the gyroscope of the sub IMU at the sub node s in the x axis, the y axis and the z axis of the carrier system is subjected to random constant value drift;
for acceleration of a sub-IMU at a sub-node s in its carrier system x-axis, y-axis and z-axisA random constant bias of the meter;
6.2 building child node transfer alignment mathematical model
(1) Establishing a system equation
The transmission alignment is carried out by adopting a position and attitude matching mode, and the system state equation of the child node s is as follows:
wherein, X s Is a state variable; f s Is a state transition matrix; g s Is a system noise matrix; w is a group of s The system noise is assumed to be zero mean Gaussian white noise; x s 、F s 、G s And W s The expression of (a) is as follows:
wherein the content of the first and second substances,
(2) establishing a measurement equation
Effective measurement vector Z of child node s s The specific expression of' is:
Z s ′＝[δψ s ′ δθ s ′ δγ s ′ δL s ′ δλ s ′ δH s ′] T s＝1,2,…,N
in the formula, delta psi s ′、δθ s ' and delta gamma s The difference values of course, pitch, roll and main node course, pitch and roll in the effective measurement vector of the child node s are respectively; delta L s ′、δλ s ' and δ H s ' are respectively the difference values of the latitude, longitude and altitude in the effective measurement vector of the sub-node s and the latitude, longitude and altitude of the main node after lever arm compensation;
the measurement equation for the child node s is:
Z s ′＝H s X s +v s
in the formula, a measuring matrix
Wherein:
wherein, T abs Attitude matrix being a child node s
The elements in the row a and the column b, a and b are 1,2 and 3; namely:
7. the method for onboard DPOS transfer alignment based on statistical confidence distance measurement bootstrapping as claimed in claims 4 and 6, wherein: in the step 1.6, transfer alignment based on Kalman filtering is carried out to estimate t k The method comprises the following steps of correcting the motion parameters of each sub-node at any moment by using the position error, the speed error and the attitude error of each sub-node, and specifically comprises the following steps:
7.1 estimating position, velocity and attitude errors of the child nodes
Based on the transfer alignment mathematical model of each child node established in claim 6, and the t obtained in claim 4 k Respectively taking the effective measurement vector of each sub-node at the moment as the measurement vector in Kalman filtering, and estimating the t of each sub-node by utilizing the Kalman filtering k Position error delta L of moment strapdown resolving s 、δλ s 、δH s Error in velocity
And angle of misalignment
Wherein s is 1,2, … N;
7.2 motion parameter correction
Correcting the strapdown resolving result of each sub-node by using the estimation result of the Kalman filtering, wherein the correction comprises position correction, speed correction and attitude correction, and the method comprises the following specific steps:
1) position correction
L s′ ＝L s -δL s ，λ s′ ＝λ s -δλ s ，H s′ ＝H s -δH s ,s＝1,2,…N
Wherein L is s′ 、λ s′ 、H s′ Are each t k The corrected latitude, longitude and altitude at the time child node s;
2) velocity correction
Wherein the content of the first and second substances,
are each t k The corrected east speed, north speed and sky speed at the time child node s;
3) attitude correction
Calculating t k Navigation coordinate system n at time child node s s And calculating a navigation coordinate system n s ' conversion matrix between
The updated attitude matrix at the child node s is:
matrix of gestures
Is recorded as:
wherein, T ab As a matrix of postures
The elements of the row a and the column b, a and b are 1,2 and 3; the main values of the course angle, the pitch angle and the roll angle after the s-th sub-node correction
Respectively as follows:
the value ranges of the course angle, the pitch angle and the roll angle are respectively defined as
The true values of the heading angle, pitch angle and roll angle are then determined by:
by for each child node t k Correcting the speed, position and attitude of the moment to obtain the speed, position and attitude information of the child node with higher precision, and finishing t k The transfers of all child nodes are aligned at time.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210746082.9A CN115127591A (en) | 2022-06-28 | 2022-06-28 | Airborne DPOS transfer alignment method based on statistical confidence distance measurement bootstrapping |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210746082.9A CN115127591A (en) | 2022-06-28 | 2022-06-28 | Airborne DPOS transfer alignment method based on statistical confidence distance measurement bootstrapping |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115127591A true CN115127591A (en) | 2022-09-30 |
Family
ID=83380488
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210746082.9A Pending CN115127591A (en) | 2022-06-28 | 2022-06-28 | Airborne DPOS transfer alignment method based on statistical confidence distance measurement bootstrapping |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115127591A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116879936A (en) * | 2023-09-07 | 2023-10-13 | 武汉大学 | INS-assisted Beidou three-frequency ambiguity initialization method and system between dynamic targets |
-
2022
- 2022-06-28 CN CN202210746082.9A patent/CN115127591A/en active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116879936A (en) * | 2023-09-07 | 2023-10-13 | 武汉大学 | INS-assisted Beidou three-frequency ambiguity initialization method and system between dynamic targets |
| CN116879936B (en) * | 2023-09-07 | 2023-11-28 | 武汉大学 | INS-assisted Beidou three-frequency ambiguity initialization method and system between dynamic targets |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112629538B (en) | Ship horizontal attitude measurement method based on fusion complementary filtering and Kalman filtering | |
| CN108387227B (en) | Multi-node information fusion method and system of airborne distributed POS | |
| CN111156987B (en) | Inertia/astronomy combined navigation method based on residual compensation multi-rate CKF | |
| CN106990426B (en) | Navigation method and navigation device | |
| CN104655152B (en) | A kind of real-time Transfer Alignments of airborne distributed POS based on federated filter | |
| US20040133346A1 (en) | Attitude change kalman filter measurement apparatus and method | |
| CN107728182B (en) | Flexible multi-baseline measurement method and device based on camera assistance | |
| CN107764261B (en) | Simulation data generation method and system for distributed POS (point of sale) transfer alignment | |
| CN112504275B (en) | Water surface ship horizontal attitude measurement method based on cascade Kalman filtering algorithm | |
| CN110243377B (en) | Cluster aircraft collaborative navigation method based on hierarchical structure | |
| CN108458709B (en) | Airborne distributed POS data fusion method and device based on vision-aided measurement | |
| CN111024124B (en) | Combined navigation fault diagnosis method for multi-sensor information fusion | |
| CN111121766A (en) | Astronomical and inertial integrated navigation method based on starlight vector | |
| CN112525191B (en) | Airborne distributed POS transfer alignment method based on relative strapdown calculation | |
| CN111189442B (en) | CEPF-based unmanned aerial vehicle multi-source navigation information state prediction method | |
| CN113291493B (en) | Method and system for determining fusion attitude of multiple sensors of satellite | |
| CN109489661B (en) | Gyro combination constant drift estimation method during initial orbit entering of satellite | |
| CN110954102A (en) | Magnetometer-assisted inertial navigation system and method for robot positioning | |
| CN112562077A (en) | Pedestrian indoor positioning method integrating PDR and prior map | |
| CN116007620A (en) | Combined navigation filtering method, system, electronic equipment and storage medium | |
| CN108303120B (en) | Real-time transfer alignment method and device for airborne distributed POS | |
| CN116222551A (en) | Underwater navigation method and device integrating multiple data | |
| CN115127591A (en) | Airborne DPOS transfer alignment method based on statistical confidence distance measurement bootstrapping | |
| CN114690229A (en) | GPS-fused mobile robot visual inertial navigation method | |
| CN111220151B (en) | Inertia and milemeter combined navigation method considering temperature model under load system |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |