CN116383966B - Multi-unmanned system distributed cooperative positioning method based on interaction multi-model - Google Patents
Multi-unmanned system distributed cooperative positioning method based on interaction multi-model Download PDFInfo
- Publication number
- CN116383966B CN116383966B CN202310331808.7A CN202310331808A CN116383966B CN 116383966 B CN116383966 B CN 116383966B CN 202310331808 A CN202310331808 A CN 202310331808A CN 116383966 B CN116383966 B CN 116383966B
- Authority
- CN
- China
- Prior art keywords
- model
- unmanned system
- unmanned
- moment
- under
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 47
- 230000003993 interaction Effects 0.000 title description 2
- 238000005259 measurement Methods 0.000 claims abstract description 46
- 230000004927 fusion Effects 0.000 claims abstract description 15
- 238000001914 filtration Methods 0.000 claims abstract description 13
- 230000002452 interceptive effect Effects 0.000 claims abstract description 12
- 239000011159 matrix material Substances 0.000 claims description 81
- 238000004364 calculation method Methods 0.000 claims description 15
- 230000007704 transition Effects 0.000 claims description 9
- 230000008569 process Effects 0.000 claims description 8
- 238000013178 mathematical model Methods 0.000 claims description 5
- 230000003190 augmentative effect Effects 0.000 claims description 4
- 238000010606 normalization Methods 0.000 claims description 4
- 125000004122 cyclic group Chemical group 0.000 claims description 3
- 230000003416 augmentation Effects 0.000 claims description 2
- 238000007796 conventional method Methods 0.000 abstract 1
- 238000004422 calculation algorithm Methods 0.000 description 7
- 238000010586 diagram Methods 0.000 description 3
- 238000012986 modification Methods 0.000 description 3
- 230000004048 modification Effects 0.000 description 3
- 238000004088 simulation Methods 0.000 description 3
- XLYOFNOQVPJJNP-UHFFFAOYSA-N water Substances O XLYOFNOQVPJJNP-UHFFFAOYSA-N 0.000 description 3
- 230000000694 effects Effects 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 238000012360 testing method Methods 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 238000005094 computer simulation Methods 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000011835 investigation Methods 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 238000013179 statistical model Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F30/00—Computer-aided design [CAD]
- G06F30/10—Geometric CAD
- G06F30/15—Vehicle, aircraft or watercraft design
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/17—Function evaluation by approximation methods, e.g. inter- or extrapolation, smoothing, least mean square method
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/18—Complex mathematical operations for evaluating statistical data, e.g. average values, frequency distributions, probability functions, regression analysis
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F30/00—Computer-aided design [CAD]
- G06F30/20—Design optimisation, verification or simulation
Abstract
The invention discloses a multi-unmanned system distributed cooperative positioning method based on an interactive multi-model, which comprises the following steps: modeling the motion state of the unmanned system by adopting a multi-model strategy, carrying out distributed filtering update on measurement information between the relative landmark and other unmanned systems by means of first-order Taylor expansion, and realizing the fusion of state estimation results under each model by utilizing interactive multi-model. The invention solves the problem of reduced or even divergent positioning precision caused by inaccurate modeling of the conventional method under the complex maneuvering condition of the unmanned system, realizes the purpose of assisting the unmanned system with high-precision positioning equipment in positioning other unmanned systems with low-precision positioning equipment, and provides a positioning method with low cost, easy expansion and high precision for the unmanned system cluster operation.
Description
Technical Field
The invention relates to the field of unmanned system distributed co-location, in particular to a multi-unmanned system distributed co-location method based on an interactive multi-model.
Background
In the application process, the reliable and accurate positioning of an unmanned system is a primary premise for completing various operations. The co-location technology of the multi-unmanned system is always the focus of research, so that the improvement of the co-location precision of the multi-unmanned system has great significance in theory and practice.
Under the closed or obstacle shielding environment, for example, when the unmanned underwater vehicle is used for cooperative operation to complete the tasks of mine removal, tracking, investigation and the like, the unmanned underwater vehicle needs to determine the information such as the position and the like of the unmanned underwater vehicle so as to facilitate the subsequent planning and control. As the GPS signal in water decays rapidly and all unmanned submarines are equipped with expensive high-precision navigation systems, the cost is high, and the adoption of the submarines equipped with high-precision navigation equipment for correcting the positioning precision of the low-precision navigation equipment by using relative observation is an economic and reliable scheme. How to fuse the relative measurement information between unmanned platforms such as unmanned submarines and the like which are all under the maneuvering motion is a great challenge for realizing the high-precision co-positioning unmanned system cluster operation, and the difficulty is how to model the complex motion and how to utilize the relative measurement to realize the tracking of other unmanned platforms to finally realize the co-positioning.
Chinese patent publication No. CN104252178B discloses a strong maneuver-based target tracking method, which uses an IMM algorithm for recalculating weights based on the IMM algorithm. The method not only utilizes the model probability, but also fully utilizes the filtering covariance matrix, so that the tracking accuracy is higher. The Chinese patent with publication number of CN102568004A discloses a high maneuvering target tracking algorithm, which tracks maneuvering targets by adopting an IMM-based Kalman filter, combines a current statistical model with acceleration self-adaptive adjustment with CV and CA models in the IMM algorithm, improves the performance of the whole IMM algorithm, calculates Markov transition probability on line in real time by utilizing system mode information hidden in current measurement, thereby obtaining more accurate posterior estimation and improving model fusion precision. The two Chinese patents provide different solutions for the problem that the unmanned system has low positioning accuracy due to strong mobility under single model modeling, but the two algorithms are only applicable to a single unmanned system and are not applicable to a plurality of unmanned systems which need to cooperate with each other to finish the operation.
The Chinese patent with publication number of CN11595348B discloses a master-slave cooperative positioning method of an autonomous underwater vehicle integrated navigation system, which is characterized in that the master AUV transmits own position information, the slave AUV acquires the relative distance with the master AUV through sound velocity and time delay, and the master AUV utilizes speed measurement information and distance measurement information to cooperatively position any slave AUV, so that the distance between the master AUV and the slave AUV is corrected, and the precision of master-slave cooperative positioning can be improved. However, in the state space modeling of the method, the state equation is modeled by using only one traditional single model, and the autonomous underwater vehicle has great mobility in actual actions, and the master-slave scheme is not suitable for the situation that the number of underwater vehicles is large.
In the above-mentioned research unmanned system co-location method, the complexity and mobility of unmanned system cluster movement are not considered at the same time, and the distributed location strategy which is easy to expand and maintain is considered at the same time.
Disclosure of Invention
The invention aims to: the invention aims to provide a multi-unmanned system distributed cooperative positioning method based on an interactive multi-model, which can realize the purpose that an unmanned system provided with high-precision positioning equipment assists other unmanned systems provided with low-precision positioning equipment to position.
The technical scheme is as follows: the invention relates to a multi-unmanned system distributed cooperative positioning method, which comprises the following steps:
s1, analyzing a motion form mathematical model of an unmanned system by utilizing a multi-model strategy, constructing a model set, and simultaneously establishing a nonlinear measurement equation of the multi-unmanned system;
s2, obtaining interactive input by using state estimation values of each unmanned system at the last moment under different motion models;
s3, after time updating is carried out on each unmanned system, filtering updating is realized by adopting a distributed structure by utilizing relative measurement of each unmanned system and measurement information of relative landmarks;
s4, updating the probability of each unmanned system corresponding to different motion models through likelihood functions;
and S5, carrying out weighted fusion on the estimation results of the unmanned systems corresponding to different models to obtain a fusion estimation result of the positioning of each unmanned system.
Further, in step S1, according to the complexity and mobility of the unmanned system, a motion form mathematical model of the unmanned system is analyzed by using a multi-model strategy and a model set is constructed, and a nonlinear measurement equation of the multi-unmanned system is established; the specific implementation steps are as follows:
step 11, constructing a model set of a motion equation of the multi-unmanned system:
wherein,is the system state variable of the ith unmanned system under the mth model at the k moment, F m (k) For the state transition matrix under the mth model at the k moment, G m (k) A system noise matrix for model m; w (w) m (k) Is zero in mean value and Q in covariance matrix m Is a process noise of (2);
step 12, modeling a nonlinear measurement equation:
wherein,for the measurement variable of the ith unmanned system under the mth model at k moment, +.>An observation function matrix of the ith unmanned system under the mth model at the k moment; v m (k) Is zero mean and covariance matrix is R m Is a measurement noise of the test piece.
Further, in step S2, a state estimate by the ith unmanned system under the nth model at time k-1Covariance matrix +.>Model probability values μ for each filter are combined n (k-1) a Markov probability transition matrix p nm Calculating to obtain a mixed state estimated value of the ith unmanned system under the mth model at the k-1 moment +.>And hybrid covariance value->Performing cyclic calculation by taking the mixed state estimation value and the mixed covariance value as initial states; the specific implementation steps are as follows:
s21, calculating the mixing probability from the model n to the model m as follows:
wherein,the probability is predicted for the model m, and the calculation formula is as follows:
wherein p is nm For the corresponding model n to the corresponding modelTransition probabilities between m; mu (mu) n (k) Is the probability of model n at time k;
s22, calculating a model m mixed state estimated value as follows:
s23, calculating a model m mixed covariance value:
where r is the total number of models and T is the transpose of the matrix.
Further, in step S3, after the time update, each unmanned system performs filtering update on the relative measurement of each other and the measurement information of the relative landmarks by means of first-order taylor expansion, and calculates respective state estimation values, error covariance matrix, innovation and innovation covariance matrix, and a semi-cross-correlation covariance matrix between unmanned systems, which specifically includes the following steps:
s31, using the hybrid state estimation value of the unmanned system iAnd a hybrid covariance matrixAnd (5) time updating:
wherein,and->The state prediction value and the state prediction error covariance matrix of the ith unmanned system under the mth model are respectively +.>The method is a semi-cross correlation covariance matrix of the unmanned system i and the unmanned system j under the mth model at the k moment, and the semi-cross correlation covariance matrix meets the following conditions:
the cross-correlation covariance matrix of the unmanned system i and the unmanned system j under the m model at the k moment;
s32, detecting the position of the landmark L, and calculating an innovation covariance matrix under the m-th model at the k moment through an actual measurement value and a predicted measurement value of the unmanned system i relative to the landmark L, wherein the calculation formula is as follows:
wherein the method comprises the steps ofIs->At->A measured jacobian matrix at the location;
s33, calculating a Kalman filtering gain of the unmanned system i relative to the landmark L under the mth model at the k moment:
s34, calculating a filter estimated value and a filter covariance matrix of the unmanned system i relative to the landmark L under the mth model at the k moment:
wherein I represents an identity matrix;
s35, detecting the position of the unmanned system j, and calculating an innovation covariance matrix under the m model at the k moment through an actual measurement value and a predicted measurement value of the unmanned system i relative to the unmanned system j, wherein the calculation formula is as follows:
wherein the method comprises the steps ofTo augment the jacobian matrix +.>Andrespectively is a measurement function h m (X m ) At->And->A measured jacobian matrix at the location;
s36, calculating the augmentation state estimation of the unmanned system i and the unmanned system j under the mth model at the k moment:
s37, calculating an augmented estimation error covariance matrix of the unmanned system i and the unmanned system j under the mth model at the k moment:
wherein the method comprises the steps of
S38, calculating a filter estimated value and a filter covariance matrix of the unmanned system i relative to the unmanned system j under the mth model at the k moment:
wherein t is the remaining unmanned systems except unmanned system i and unmanned system j;
further, in step S4, the probability that each unmanned system corresponds to a different motion model is updated, and the specific implementation steps are as follows:
s41, updating the model probability at the moment k through a likelihood function, wherein the likelihood function of the model m is as follows:
wherein,the new information covariance matrix and the new information covariance matrix of the unmanned system i under the mth model at the k moment are respectively, and x is the dimension of a state variable;
s42, the probability of the model m is:
wherein c is a normalization constant,
further, in step S5, each unmanned system performs weighted fusion on the estimation results corresponding to different models, so as to obtain a fusion estimation result of positioning of each unmanned system, and the specific implementation steps are as follows:
s51, based on the model probability value corresponding to each filter, obtaining a total state estimated value by weighting calculation on the estimated value of each filter of the unmanned system i at the moment:
s52, calculating an overall covariance estimation value of the unmanned system i:
compared with the prior art, the invention has the following remarkable effects:
1. the invention provides a multi-unmanned system distributed cooperative positioning method considering switching of different motion models, which is characterized in that a multi-model method is adopted to model the motion state of an unmanned system, distributed filtering updating is carried out on measurement information between a relative landmark and other unmanned systems by means of first-order Taylor expansion, and the fusion of state results under each model is realized by utilizing an interactive multi-model method, so that the optimal state estimation of each unmanned system is obtained;
2. the invention adopts a distributed scheme to provide reliable positioning information for the cluster operation of the multi-unmanned system, realizes the purpose that the unmanned system provided with high-precision positioning equipment assists other unmanned systems provided with low-precision positioning equipment in positioning, has the characteristics of low cost, strong expansibility, high estimation precision and the like, can improve the positioning precision of the multi-unmanned system in the environment where radio signals such as underground, indoor, underwater and the like are interfered, and ensures the collaborative operation task of the multi-unmanned system.
Drawings
FIG. 1 is a schematic flow chart of an algorithm of the present invention;
FIG. 2 is a diagram of simulation results of the present invention;
fig. 3 is a schematic diagram of a practical application scenario of the collaborative operation in the underwater multi-unmanned system.
Detailed Description
The invention is described in further detail below with reference to the drawings and the detailed description.
As shown in fig. 1, the distributed co-location method of the multi-unmanned system based on the interactive multi-model specifically includes the following steps:
step 1, unmanned system modeling
According to the motion complexity and mobility of the unmanned system, a mathematical model of a possible motion form of the unmanned system is analyzed by utilizing a multi-model strategy, a model set is constructed, and a nonlinear measurement equation of the multi-unmanned system is established; the specific method comprises the following steps:
step 11, constructing a model set of a motion equation of the multi-unmanned system:
wherein,is the system state variable of the ith unmanned system under the mth model at the k moment, F m (k) For the state transition matrix under the mth model at the k moment, G m (k) A system noise matrix for model m; w (w) m (k) Is zero in mean value and Q in covariance matrix m Is a process noise of (a).
Step 12, modeling a nonlinear measurement equation:
wherein,for the measurement variable of the ith unmanned system under the mth model at k moment, +.>An observation function matrix of the ith unmanned system under the mth model at the k moment; v m (k) Is zero mean and covariance matrix is R m Is a measurement noise of the test piece.
Step 2, model fusion input
And obtaining interactive input by using the state estimation value of each unmanned system at the last moment under different motion models. State estimation by the ith unmanned system under model n at time k-1Covariance matrixModel probability values μ for each filter are combined n (k-1) a Markov probability transition matrix p nm Calculating to obtain mixed state estimated value +.>And hybrid covariance value->And taking the mixed state estimation value and the mixed covariance value as initial states to carry out cyclic calculation. The specific calculation is as follows:
step 21, calculating the mixing probability of the model n to the model m as follows:
in the method, in the process of the invention,the prediction probability (normalization constant) of the model m is calculated as:
wherein r is the total number of models, p nm The transition probability from the corresponding model n to the corresponding model m; mu (mu) n (k) Is the probability of model n at time k;
step 22, calculating the model m mixed state estimation value as follows:
step 23, calculating a model m hybrid covariance value as follows:
where "T" represents the matrix transpose.
Step 3, model condition filtering
After time updating, each unmanned system realizes filtering updating on relative measurement of each other and measurement information of relative landmarks by means of first-order Taylor expansion by adopting a distributed structure, and calculates respective state estimation values, error covariance matrixes, innovation and innovation covariance matrixes and semi-cross-correlation covariance matrixes among the unmanned systems, wherein the specific method comprises the following steps:
step 31, using the hybrid state estimation value of the unmanned system iAnd a hybrid covariance matrixAnd (5) time updating:
wherein,the method is a semi-cross correlation covariance matrix of the unmanned system i and the unmanned system j under the mth model at the k moment, and the semi-cross correlation covariance matrix meets the following conditions:
the cross-correlation covariance matrix of the unmanned system i and the unmanned system j under the mth model at the k moment.
Step 32, detecting the position of the landmark L, and calculating an innovation and innovation covariance matrix under the k moment m model by the unmanned system i relative to the actual measurement value and the predicted measurement value of the landmark L, wherein the calculation formula is as follows:
wherein,for measuring jacobian matrix.
Step 33, calculating a Kalman filtering gain of the unmanned system i relative to the landmark L under the mth model at the k moment:
step 34, calculating a filter estimated value and a filter covariance matrix of the unmanned system i relative to the landmark L under the mth model at the k moment:
wherein I represents an identity matrix.
Step 35, detecting the position of the unmanned system j, and calculating an innovation covariance matrix under the m-th model at the k moment by using the actual measurement value and the predicted measurement value of the unmanned system i relative to the unmanned system j, wherein the calculation formula is as follows:
wherein the method comprises the steps ofTo augment the jacobian matrix +.>Andrespectively is a measurement function h m (X m ) At->And->A measured jacobian matrix at the location;
step 36, calculating the augmented state estimation of the unmanned systems i and j under the mth model at the k moment:
step 37, calculating an augmented estimation error covariance matrix of the unmanned systems i and j under the mth model at the k moment:
wherein the method comprises the steps of
Step 38, calculating a filter estimated value and a filter covariance matrix of the unmanned system i relative to the unmanned system j under the mth model at the k moment:
wherein t is the remaining unmanned systems except unmanned system i and unmanned system j;
step 4, updating the model probability
The probability of each unmanned system corresponding to different motion models is updated, and the specific method comprises the following steps:
step 41, updating the model probability at the time k through a likelihood function, wherein the likelihood function of the model m is:
wherein,and->The new information covariance matrix and the new information covariance matrix of the unmanned system i under the mth model at the k moment are respectively, and x is the dimension of a state variable;
in step 42, the probability of model m is:
wherein c is a normalization constant,
step 5, model fusion output
Each unmanned system carries out weighted fusion on the estimation results corresponding to different models to obtain a fusion estimation result of each unmanned system positioning, and the specific method comprises the following steps:
step 51, based on the model probability value corresponding to each filter, obtaining a total state estimated value for the estimated value of each filter of the unmanned system i at the moment through weighted calculation:
step 52, calculating the total covariance estimate of the unmanned system i:
the effectiveness of the present invention is further verified by specific examples as follows:
in order to verify the effect of the method provided by the invention, four unmanned systems and three motion models are selected to construct a multi-unmanned system co-location system, namely r=3.
And selecting a second-order constant speed model and a second-order coordinated turning model to design a process equation model set:
wherein Δt is the sampling time interval; f (F) 1 Is a second-order constant speed model;
wherein ω is the turn rate; f (F) 2 Is a second-order coordinated turning model;
modeling a nonlinear metrology equation using distance and orientation:
in the method, in the process of the invention,for the position of the unmanned system i at time k, < >>Is the location of a landmark, radar or other unmanned system;
in the simulation, the embodiment sets the model initialization probability of each unmanned system to μ 0 ＝[0.3,0.3,0.4] T The initial state transition matrix isThe course of motion of each unmanned system is shown in table 1:
table 1 four unmanned system maneuver state tables
Note that: CV is uniform linear motion; CT_ (+2) is a left-turn motion with a turning rate of 2 DEG/s, similarly CT_ (-2) is a right-turn motion with a turning rate of 2 DEG/s, and CT_ (+3), CT_ (+4), CT_ (+5) and CT_ (-3), CT_ (-4), CT_ (-3.5) are analogically.
Fig. 2 is a simulation result diagram of the method proposed by the present invention, where U1t represents a real track of the unmanned system 1, U1e represents a track after filtering by the unmanned system 1, and so on for U2t, U3t, U4t, and U2e, U3e, and U4e. The result shows that the invention can achieve better positioning accuracy for the multi-unmanned system co-positioning system with multiple movements.
As shown in fig. 3, in the practical application scenario of the embodiment, the radio is not utilized underwater due to attenuation of the electromagnetic wave signal under water, and one or more unmanned submarines carry high-precision positioning devices or float to the water surface at regular time to perform state correction, so that the positioning precision of the cooperative system is improved.
It will be apparent to those skilled in the art that various modifications and variations can be made to the present invention without departing from the spirit or scope of the invention. Thus, if such modifications and variations of the present invention fall within the scope of the equivalent technique of the present invention, the present invention is intended to include such modifications and variations.
Claims (3)
1. A multi-unmanned system distributed cooperative positioning method based on an interactive multi-model is characterized by comprising the following steps:
s1, analyzing a motion form mathematical model of an unmanned system by utilizing a multi-model strategy, constructing a model set, and simultaneously establishing a nonlinear measurement equation of the multi-unmanned system;
s2, obtaining interactive input by using state estimation values of each unmanned system at the last moment under different motion models;
s3, after time updating is carried out on each unmanned system, filtering updating is realized by adopting a distributed structure by utilizing relative measurement of each unmanned system and measurement information of relative landmarks;
s4, updating the probability of each unmanned system corresponding to different motion models through likelihood functions;
s5, carrying out weighted fusion on the estimation results of the unmanned systems corresponding to different models to obtain a fusion estimation result of the unmanned system positioning;
in step S1, according to the complexity and mobility of the unmanned system, analyzing a motion form mathematical model of the unmanned system by utilizing a multi-model strategy and constructing a model set, and simultaneously, establishing a nonlinear measurement equation of the multi-unmanned system; the specific implementation steps are as follows:
step 11, constructing a model set of a motion equation of the multi-unmanned system:
wherein,is the system state variable of the ith unmanned system under the mth model at the k moment, F m (k) For the state transition matrix under the mth model at the k moment, G m (k) A system noise matrix for model m; w (w) m (k) Is zero in mean value and Q in covariance matrix m Is a process noise of (2);
step 12, modeling a nonlinear measurement equation:
wherein,for the measurement variable of the ith unmanned system under the mth model at k moment，/>An observation function matrix of the ith unmanned system under the mth model at the k moment; v m (k) Is zero mean and covariance matrix is R m Is a measurement noise of (1);
in step S2, a state estimate is made by the ith unmanned system in the nth model at time k-1Covariance matrix +.>Model probability values μ for each filter are combined n (k-1) a Markov probability transition matrix p nm Calculating to obtain a mixed state estimated value of the ith unmanned system under the mth model at the k-1 moment +.>And hybrid covariance value->Performing cyclic calculation by taking the mixed state estimation value and the mixed covariance value as initial states; the specific implementation steps are as follows:
s21, calculating the mixing probability from the model n to the model m as follows:
wherein,the probability is predicted for the model m, and the calculation formula is as follows:
wherein p is nm For the corresponding mouldTransition probability between type n to corresponding model m; mu (mu) n (k) Is the probability of model n at time k;
s22, calculating a model m mixed state estimated value as follows:
s23, calculating a model m mixed covariance value:
wherein r is the total number of models, and T is the transpose of the matrix;
in step S3, after time update, each unmanned system performs filtering update on the relative measurement of each other and the measurement information of the relative landmarks by means of first-order taylor expansion, calculates respective state estimation values, error covariance matrix, innovation, and innovation covariance matrix, and a semi-cross-correlation covariance matrix between unmanned systems, and specifically includes the following implementation steps:
s31, using the hybrid state estimation value of the unmanned system iAnd a hybrid covariance matrixAnd (5) time updating:
wherein,and->Respectively a state prediction value and a state prediction error covariance matrix of the ith unmanned system under the mth model, Q m (k) For the process noise covariance matrix under the mth model at time k, +.>The method is a semi-cross correlation covariance matrix of the unmanned system i and the unmanned system j under the mth model at the k moment, and the semi-cross correlation covariance matrix meets the following conditions:
the cross-correlation covariance matrix of the unmanned system i and the unmanned system j under the m model at the k moment;
s32, detecting the position of the landmark L, and calculating an innovation covariance matrix under the m-th model at the k moment through an actual measurement value and a predicted measurement value of the unmanned system i relative to the landmark L, wherein the calculation formula is as follows:
wherein the method comprises the steps ofIs->At->Measured jacobian matrix at R m (k) The measured noise covariance matrix is the m-th model at the k moment;
s33, calculating a Kalman filtering gain of the unmanned system i relative to the landmark L under the mth model at the k moment:
s34, calculating a filter estimated value and a filter covariance matrix of the unmanned system i relative to the landmark L under the mth model at the k moment:
wherein I represents an identity matrix;
s35, detecting the position of the unmanned system j, and calculating an innovation covariance matrix under the m model at the k moment through an actual measurement value and a predicted measurement value of the unmanned system i relative to the unmanned system j, wherein the calculation formula is as follows:
wherein the method comprises the steps ofTo augment the jacobian matrix +.>Andrespectively is a measurement function h m (X m ) At->And->A measured jacobian matrix at the location;
s36, calculating the augmentation state estimation of the unmanned system i and the unmanned system j under the mth model at the k moment:
s37, calculating an augmented estimation error covariance matrix of the unmanned system i and the unmanned system j under the mth model at the k moment:
wherein the method comprises the steps of
S38, calculating a filter estimated value and a filter covariance matrix of the unmanned system i relative to the unmanned system j under the mth model at the k moment:
where t is the remaining unmanned systems except unmanned system i and unmanned system j.
2. The distributed co-location method of multiple unmanned systems based on interactive multiple models according to claim 1, wherein in step S4, updating the probability of each unmanned system corresponding to a different motion model is performed by:
s41, updating the model probability at the moment k through a likelihood function, wherein the likelihood function of the model m is as follows:
wherein,the new information covariance matrix and the new information covariance matrix of the unmanned system i under the mth model at the k moment are respectively, and x is the dimension of a state variable;
s42, the probability of the model m is:
wherein,for the predictive probability of the ith unmanned system model m, c is the normalization constant, ++>
3. The multi-unmanned system distributed cooperative positioning method based on the interactive multi-model according to claim 2, wherein in step S5, each unmanned system performs weighted fusion on the estimation results of the corresponding different models to obtain a fusion estimation result of each unmanned system positioning, and the specific implementation steps are as follows:
s51, based on the model probability value corresponding to each filter, obtaining a total state estimated value by weighting calculation on the estimated value of each filter of the unmanned system i at the moment:
s52, calculating an overall covariance estimation value of the unmanned system i:
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310331808.7A CN116383966B (en) | 2023-03-30 | 2023-03-30 | Multi-unmanned system distributed cooperative positioning method based on interaction multi-model |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310331808.7A CN116383966B (en) | 2023-03-30 | 2023-03-30 | Multi-unmanned system distributed cooperative positioning method based on interaction multi-model |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116383966A CN116383966A (en) | 2023-07-04 |
| CN116383966B true CN116383966B (en) | 2023-11-21 |
Family
ID=86974405
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310331808.7A Active CN116383966B (en) | 2023-03-30 | 2023-03-30 | Multi-unmanned system distributed cooperative positioning method based on interaction multi-model |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116383966B (en) |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103697889A (en) * | 2013-12-29 | 2014-04-02 | 北京航空航天大学 | Unmanned aerial vehicle self-navigation and positioning method based on multi-model distributed filtration |
| CN107193009A (en) * | 2017-05-23 | 2017-09-22 | 西北工业大学 | A kind of many UUV cooperative systems underwater target tracking algorithms of many interaction models of fuzzy self-adaption |
| CN109655826A (en) * | 2018-12-16 | 2019-04-19 | 成都汇蓉国科微系统技术有限公司 | The low slow Small object track filtering method of one kind and device |
| WO2021082571A1 (en) * | 2019-10-29 | 2021-05-06 | 苏宁云计算有限公司 | Robot tracking method, device and equipment and computer readable storage medium |
| CN114035154A (en) * | 2021-11-10 | 2022-02-11 | 中国人民解放军空军工程大学 | Motion parameter assisted single-station radio frequency signal positioning method |
-
2023
- 2023-03-30 CN CN202310331808.7A patent/CN116383966B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103697889A (en) * | 2013-12-29 | 2014-04-02 | 北京航空航天大学 | Unmanned aerial vehicle self-navigation and positioning method based on multi-model distributed filtration |
| CN107193009A (en) * | 2017-05-23 | 2017-09-22 | 西北工业大学 | A kind of many UUV cooperative systems underwater target tracking algorithms of many interaction models of fuzzy self-adaption |
| CN109655826A (en) * | 2018-12-16 | 2019-04-19 | 成都汇蓉国科微系统技术有限公司 | The low slow Small object track filtering method of one kind and device |
| WO2021082571A1 (en) * | 2019-10-29 | 2021-05-06 | 苏宁云计算有限公司 | Robot tracking method, device and equipment and computer readable storage medium |
| CN114035154A (en) * | 2021-11-10 | 2022-02-11 | 中国人民解放军空军工程大学 | Motion parameter assisted single-station radio frequency signal positioning method |
Non-Patent Citations (1)
| Title |
|---|
| 一种面向多无人机协同感知的分布式融合估计方法;王林;王楠;朱华勇;沈林成;;控制与决策(第06期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116383966A (en) | 2023-07-04 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Shen et al. | Dual-optimization for a MEMS-INS/GPS system during GPS outages based on the cubature Kalman filter and neural networks | |
| CN109459040B (en) | Multi-AUV (autonomous Underwater vehicle) cooperative positioning method based on RBF (radial basis function) neural network assisted volume Kalman filtering | |
| CN109521454A (en) | A kind of GPS/INS Combinated navigation method based on self study volume Kalman filtering | |
| Schon et al. | Marginalized particle filters for mixed linear/nonlinear state-space models | |
| Mu et al. | A practical INS/GPS/DVL/PS integrated navigation algorithm and its application on Autonomous Underwater Vehicle | |
| CN110779518B (en) | Underwater vehicle single beacon positioning method with global convergence | |
| CN102622520B (en) | A kind of distributed multimode type estimation fusion method of maneuvering target tracking | |
| CN106772524B (en) | A kind of agricultural robot integrated navigation information fusion method based on order filtering | |
| CN109631913A (en) | X-ray pulsar navigation localization method and system based on nonlinear prediction strong tracking Unscented kalman filtering | |
| CN105509739A (en) | Tightly coupled INS/UWB integrated navigation system and method adopting fixed-interval CRTS smoothing | |
| CN109974706A (en) | A kind of more AUV collaborative navigation methods of master-slave mode based on double motion models | |
| CN105116431A (en) | Inertial navigation platform and Beidou satellite-based high-precision and ultra-tightly coupled navigation method | |
| Zhang et al. | A hybrid intelligent algorithm DGP-MLP for GNSS/INS integration during GNSS outages | |
| CN110779519B (en) | Underwater vehicle single beacon positioning method with global convergence | |
| CN101701826A (en) | Target tracking method of passive multi-sensor based on layered particle filtering | |
| Johansen et al. | Three-stage filter for position estimation using pseudorange measurements | |
| CN113325452A (en) | Method for tracking maneuvering target by using three-star passive fusion positioning system | |
| CN107607977A (en) | A kind of adaptive UKF Combinated navigation methods based on the sampling of minimum degree of bias simple form | |
| Bryson et al. | Co-operative localisation and mapping for multiple UAVs in unknown environments | |
| CN111025229B (en) | Underwater robot pure orientation target estimation method | |
| Jingsen et al. | Integrating extreme learning machine with Kalman filter to bridge GPS outages | |
| Xu et al. | Accurate two-step filtering for AUV navigation in large deep-sea environment | |
| CN115933641A (en) | AGV path planning method based on model prediction control guidance deep reinforcement learning | |
| Girija et al. | Tracking filter and multi-sensor data fusion | |
| Saadeddin et al. | Optimization of intelligent-based approach for low-cost INS/GPS navigation 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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |