Multi-aircraft cooperative positioning method based on robust message passing
By constructing a multi-aircraft collaborative positioning method based on robust messaging, using double-difference GNSS and UWB observations to model and using Cauchy loss function, the impact of anomaly observations is minimized, and the problem of deterioration in the accuracy of GNSS observations in urban areas is solved, and distributed positioning with high reliability and robustness is achieved.
Patent Information
- Application Number
- CN202510560517.4
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-04-30
- Publication Date
- 2025-08-08
AI Technical Summary
The prior art GNSS observations in urban areas are susceptible to non-sight range and multi-path environments, resulting in deterioration of positioning accuracy. It is difficult for conventional robust collaborative positioning methods to balance low false alarm rate and low leakage detection rate, and the Huber loss function has a slow impact on large error observation.
By constructing a multi-aircraft collaborative positioning method based on robust messaging, double-difference GNSS and UWB observation modeling are used, weighted matrix is set up to correct the noise, and the Cauchy loss function is used to minimize the impact of anomaly observation, realizing distributed collaborative positioning.
Effectively suppress the impact of large errors on positioning accuracy in complex and high noise environments, improve the reliability and robustness of positioning, and realize distributed robust collaborative positioning.
Smart Images
Figure CN120447007A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to a multi-aircraft collaborative positioning method based on robust message transmission, and belongs to the technical field of navigation and positioning. Background Art
[0002] Aircraft, including unmanned aerial vehicles (UAVs) and vertical take-off and landing vehicles (vTOLs), have been widely used in civil and military fields such as logistics distribution, transport and rescue. High-precision positioning is a prerequisite for tasks such as aircraft perception, planning and control, and is crucial to aircraft. Existing Global Navigation Satellite System (GNSS) solutions, such as precise point positioning (PPP) and real-time differential positioning (RTD), are susceptible to non-line-of-sight and multipath environments in urban areas, resulting in observation anomalies and deterioration of positioning accuracy. Through inter-aircraft collaboration, adjacent aircraft can communicate and measure relative to each other, and by differentiating GNSS observations to eliminate common-mode errors and obtain additional observations, positioning accuracy can be improved. Therefore, collaborative positioning is expected to provide continuous, accurate and reliable position information for aircraft.
[0003] However, unlike conventional RTD positioning, collaborative positioning requires differential GNSS observations between aircraft whose positions are unknown, leading to coupling between the estimated parameters of different aircraft. Furthermore, factors such as non-line-of-sight, sensor failures, and platform vibrations can cause nonideal errors in the observations obtained by the aircraft, leading to anomalies in some of the observations. Therefore, research on robust collaborative positioning is needed to improve positioning reliability and robustness.
[0004] Commonly used robust collaborative localization methods include fault detection and elimination (FDE) and M-estimation. FDE judges observations by constructing detection statistics and improves positioning robustness by eliminating abnormal observations. However, FDE has difficulty striking a balance between low false alarm rate and low missed detection rate. M-estimation changes the weights of observations to minimize the robust loss function to reduce the impact of abnormal observations on positioning. Existing studies have all improved robustness by minimizing the Huber loss function. However, the Huber loss applies a quadratic penalty to small errors and a linear penalty to large errors. When the observation results are significantly abnormal, the linear penalty of the Huber loss causes the weight of the outlier observations to slowly decrease, making it difficult to reduce the impact of large error observations on positioning. Summary of the Invention
[0005] The purpose of the present invention is to provide a multi-aircraft collaborative positioning method based on robust message passing, which can effectively suppress the influence of large errors on positioning accuracy and perform better in complex and high-noise environments.
[0006] The technical solutions for implementing the present invention are as follows:
[0007] A multi-aircraft collaborative localization method based on robust message passing is proposed. The specific process is as follows:
[0008] Step 1: Co-positioning observation modeling: double-difference GNSS observation modeling and obtain its relief function, UWB observation modeling and obtain its relief function;
[0009] Step 2: Robust positioning model construction: Construct a collaborative positioning model based on the likelihood function of double-difference GNSS observations and the likelihood function of UWB observations;
[0010] Step 3: Distributed collaborative positioning: Set the weighting matrix to respectively adjust the noise vector of the double-difference GNSS observation in the collaborative positioning model. The covariance matrix of Correction and UWB observation noise Variance Corrections are made and the corrected positioning model is iteratively solved to achieve collaborative positioning of multiple aircraft.
[0011] Optionally, in each iteration of collaborative positioning, the present invention performs message interaction between adjacent aircraft and aggregates messages from different aircraft to achieve an approximate estimation of their own global edge posteriors, thereby achieving multi-aircraft collaborative positioning.
[0012] Optionally, the present invention sets a weighted matrix W ij To achieve fault tolerance of observations;
[0013]
[0014] Among them, Λ ij is the triangular matrix obtained by Cholesky decomposition of the augmented error vector covariance matrix, is the noise vector of the double-difference GNSS observation after correction The covariance matrix of is the corrected UWB observation noise The variance of is the corrected position of aircraft i in the kth iteration.
[0015] Optionally, the weighting matrix of the present invention is:
[0016]
[0017] Among them, Δ l is a vector The lth element, φ(Δ l ) is the function β(Δ l ), diag represents the diagonal elements of the matrix;
[0018]
[0019] Where η represents the scaling parameter, δp i represents the estimated error of the aircraft position, represents the combined noise vector of double-difference GNSS observations and UWB observations, Λ ij is the triangular matrix obtained by Cholesky decomposition of the augmented error vector covariance matrix.
[0020] Optionally, the double-difference GNSS observation likelihood function of the present invention is:
[0021]
[0022] in, represents a Gaussian distribution, represents the double-difference GNSS observations between aircraft i and aircraft j and common-view satellites m and n, h DD (p i ,p j ) represents the mapping function from the positions of aircraft i and aircraft j to double-difference GNSS observations; h UWB (p i ,p j ) represents the mapping function from the positions of aircraft i and aircraft j to UWB observations, d ij represents the UWB ranging observation between adjacent aircraft i and aircraft j.
[0023] Optionally, in the k+1th iteration of the present invention, the aircraft's own global edge posterior estimate is:
[0024]
[0025] b k (p i )=f(p i )
[0026] Among them, f(p j ) represents the prior information of the position of aircraft i.
[0027] Optionally, in the present invention, when the iteration converges, each aircraft can obtain a posterior estimation of the position edge of the current positioning epoch, thereby realizing distributed robust collaborative positioning.
[0028] Optionally, the iterative convergence described in the present invention is: the difference between the posterior estimates of the position edges of two adjacent positioning epochs is less than a set threshold.
[0029] Beneficial effects:
[0030] This collaborative localization method for mobile unmanned systems, based on robust message passing, considers the coupled state errors between aircraft and the correlated noise introduced by collaborative differencing. It constructs a global joint a posteriori probability of all aircraft positions, or collaborative localization model, and then factorizes this probability. Next, a weighting matrix is set based on a fused Cauchy loss function. The algorithm considers the correlation between the estimated states of different aircraft and effectively identifies and mitigates the impact of anomalous observations. This enables each aircraft in the message-passing distributed collaborative localization algorithm to independently estimate the global a posteriori probability of its position in a distributed manner, while also being tolerant to anomalous measurements. BRIEF DESCRIPTION OF THE DRAWINGS
[0031] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the following briefly introduces the drawings required for use in the embodiments. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.
[0032] Figure 1 Flowchart of a multi-aircraft collaborative localization method based on robust message passing. DETAILED DESCRIPTION
[0033] The embodiments of the present invention are described in detail below with reference to the accompanying drawings.
[0034] It should be noted that, in the absence of conflict, the following embodiments and features in the embodiments may be combined with each other; and, based on the embodiments in this disclosure, all other embodiments obtained by persons of ordinary skill in the art without creative work are within the scope of protection of this disclosure.
[0035] It should be noted that various aspects of the embodiments within the scope of the appended claims are described below. It should be apparent that the aspects described herein can be embodied in a wide variety of forms, and any specific structure and / or function described herein is merely illustrative. Based on this disclosure, it should be understood by those skilled in the art that an aspect described herein can be implemented independently of any other aspect, and two or more of these aspects can be combined in various ways. For example, any number of aspects described herein can be used to implement an apparatus and / or practice a method. In addition, other structures and / or functionalities other than one or more of the aspects described herein can be used to implement this apparatus and / or practice this method.
[0036] The present invention provides a mobile unmanned system collaborative positioning method based on robust message transmission, the structural block diagram is as follows Figure 1 As shown in the figure, it includes three steps: collaborative positioning observation modeling, robust positioning problem construction and distributed collaborative positioning.
[0037] Without loss of generality, the multi-aircraft system is described as follows: Assume a mobile unmanned system consisting of M mobile aircraft. Each aircraft is equipped with a GNSS receiver that can receive multi-frequency, multi-constellation GNSS signals. In addition, each aircraft is equipped with a UWB sensor that can perform relative measurements and communicate with adjacent aircraft. The effective range of UWB is d th To locate the epoch at t, use To represent the connected set of all aircraft and satellites, use To represent the set of connections between aircraft.
[0038] For vehicle i, use the set To represent the set of satellites that are commonly seen by aircraft i and aircraft j. Set represents the set of adjacent aircraft visible to aircraft i. The position of aircraft i is expressed as
[0039] (1) Co-location Observation Modeling
[0040] A. Double-difference GNSS observation modeling
[0041] Pseudorange observation ρ between aircraft i and satellite n i n Can be modeled as
[0042]
[0043] Among them, r i n =‖p i -s n ‖ represents the actual distance between aircraft i and satellite n. s n is the position of satellite n at the time of sending the navigation signal. i represents the clock difference between the spacecraft and the satellite constellation, δt n represents the clock difference between satellite n and the satellite constellation, and T i n Indicates the measurement deviation caused by the delay of ionosphere and troposphere. i and g n The hardware chip delay of the aircraft and satellite, represents Gaussian measurement noise.
[0044] Through communication between aircraft, raw GNSS observations can be shared. Through cooperative double-difference, most common-mode errors from satellites, receivers, and signal propagation paths can be eliminated. The double-difference GNSS observations of aircraft i and j with common-view satellites m and n are as follows:
[0045]
[0046] in, is the double difference operation, is the true double difference distance, which can be written as
[0047]
[0048] Defining variables is the vector of all double-difference observations between aircraft i and aircraft j, represents the double-difference observation noise vector. Furthermore, the likelihood function of the double-difference GNSS observation between aircraft i and aircraft j can be expressed as:
[0049]
[0050] Among them, h DD (p i ,p j ) represents the mapping function from the positions of aircraft i and aircraft j to double-difference GNSS observations. The noise vector of double-difference GNSS observations is The covariance matrix of Since different double-difference GNSS observations use the same reference observation for difference, the noises of different double-difference GNSS observations are coupled with each other, and the covariance matrix is a non-diagonal matrix.
[0051] B. UWB Observation Modeling
[0052] For adjacent aircraft i and aircraft j, the UWB ranging observation quantity d ij It can be expressed as
[0053]
[0054] in, is Gaussian measurement noise. Based on (7), the UWB observation likelihood function can be expressed as
[0055]
[0056] Among them, h UWB (p i ,p j ) represents the mapping function from the positions of aircraft i and aircraft j to UWB observations. UWB observation noise The variance of
[0057] (2) Construction of robust positioning problem
[0058] Unlike conventional GNSS RTD positioning, in collaborative positioning, GNSS observations are double-differenced between aircraft whose positions are unknown, resulting in the coupling of the estimated states of different aircraft. Therefore, in order to achieve the best estimate, collaborative positioning requires estimating the positions of all aircraft. The joint posterior distribution f(p|z) of
[0059]
[0060] in, represents the prior information of the position of aircraft i, is the combined vector of all double-difference GNSS observations and UWB observations.
[0061] Based on the joint posterior distribution f(p|z) of all aircraft positions, the global marginal posterior f(p i |z) can be obtained by integrating f(p|z) with respect to all other irrelevant variables. One implementation involves centralized computation at a central node, but this results in extremely high computational complexity. In contrast, a distributed approach can reduce node computational complexity and enhance the scalability of mobile aircraft networks. Therefore, this invention proposes a fault-tolerant distributed positioning solution.
[0062] (3) Distributed collaborative positioning
[0063] The distributed collaborative localization method of the present invention is based on a message-passing framework. It is an iterative implementation whereby adjacent aircraft exchange messages in each iteration and aggregate messages from different aircraft to achieve an approximate estimate of their own global edge posteriors.
[0064] Assume that in the kth iteration, the posterior estimation result of any aircraft i on its own position is in, are the position estimation mean and covariance respectively. For the initial iteration (k=0), the prior information is used as the posterior estimation result, then b k (p i )=f(p i ). In the k+1th iteration, the posterior estimation result of any aircraft j’s own position is calculated as
[0065]
[0066] in, is the message sent by aircraft i and aircraft j, and is calculated as
[0067]
[0068] In order to achieve fault tolerance for abnormal observations, the target method of the present invention adaptively changes the likelihood function f(z ij |p i ,p j ) and the likelihood function f(d ij |p i ,p j To achieve this, first rewrite Equation (9) as the following nonlinear regression problem:
[0069]
[0070] Among them, δp i It represents the estimated error of the aircraft position in the kth iteration. By mapping the double-difference GNSS observations to the function h DD (p i ,p j ) and the UWB observation mapping function h UWB (p i ,p j ) Linearize the position of aircraft i and we can get the approximate result of formula (10):
[0071]
[0072] in, Combine the linearized vector for the double-difference GNSS and UWB observations, and is the Jacobian matrix of the observations with respect to the positions of aircraft i and aircraft j, The noise vector of the double-difference GNSS observation and the UWB observation is combined, specifically
[0073]
[0074] in, is the line-of-sight vector between aircraft i and satellite m.
[0075] When all observations are ideal, the augmented error vector Has statistical properties:
[0076]
[0077] in is the mean operation, Cov(·) is the covariance operation. ij is the triangular matrix obtained by Cholesky decomposition of the augmented error vector covariance matrix. Multiply both ends of (11) by The regression model after error normalization can be obtained:
[0078]
[0079] in,
[0080]
[0081] After obtaining the regression model after error normalization, the embodiment of the present application aims to minimize the Cauchy loss function of the regression model to have fault tolerance for abnormal observations. The Cauchy loss function can be expressed as
[0082]
[0083] Among them, Δ l is a vector The lth element of yes Model.
[0084] β is the Cauchy function, defined as
[0085]
[0086] Here, η is a scaling parameter.
[0087] The present invention adaptively changes the double-difference GNSS observation likelihood function f(z ij |p i ,p j ) and the likelihood function f(d ij |p i ,p j ), to achieve this, the derivative of the loss function (16) must be zero, that is,
[0088]
[0089] Among them, φ(Δ l ) is the function β(Δ l ). By calculating φ(Δ l ) is converted to obtain the Cauchy density function ψ(Δ l ):
[0090]
[0091] Based on (19), the present invention adopts the weighting matrix W ij =diag{ψ(Δ l )} to achieve error tolerance of the observation. If the normalized residual of the observation exceeds the threshold, it indicates that the measurement is abnormal and its weight will be reduced. Matrix W ij The weighting matrix can be used to adjust the covariance matrix of the observation error, that is:
[0092]
[0093] Based on (20), the adjusted double-difference GNSS observation likelihood function f(z ij |p i ,p j ) and the likelihood function f(d ij |p i ,p j ) can be expressed as
[0094]
[0095] Based on (21), the message (9) sent by aircraft i to aircraft j can be expressed as
[0096]
[0097] in, is the adjusted observation error covariance matrix, and is the mean and variance of the message sent from aircraft i to aircraft j, that is,
[0098]
[0099] Substituting (22) into (8), we can obtain the posterior estimation result of the global edge of the aircraft in the k+1th iteration:
[0100]
[0101] As a result, the aircraft achieves its own global edge posterior estimation result in the k+1th iteration. When the iteration converges, each aircraft can obtain the edge posterior estimation of the position of the current positioning epoch, thus achieving distributed robust collaborative positioning.
[0102] In summary, the above are only preferred embodiments of the present invention and are not intended to limit the scope of protection of the present invention. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.
Claims
1. A multi-aircraft collaborative positioning method based on robust message passing, characterized in that: The specific process is: Step 1: Co-positioning observation modeling: double-difference GNSS observation modeling and obtain its relief function, UWB observation modeling and obtain its relief function; Step 2: Robust positioning model construction: Construct a collaborative positioning model based on the likelihood function of double-difference GNSS observations and the likelihood function of UWB observations; Step 3: Distributed collaborative positioning: Set the weighting matrix to respectively adjust the noise vector of the double-difference GNSS observation in the collaborative positioning model. The covariance matrix of Correction and UWB observation noise Variance Corrections are made and the corrected positioning model is iteratively solved to achieve collaborative positioning of multiple aircraft.
2. The multi-aircraft collaborative positioning method based on robust message passing according to claim 1 is characterized in that: In each iteration of collaborative localization, adjacent aircraft exchange messages and aggregate messages from different aircraft to achieve an approximate estimation of their own global edge posteriors and realize multi-aircraft collaborative localization.
3. The multi-aircraft collaborative positioning method based on robust message passing according to claim 1 is characterized in that: Set the weight matrix W ij To achieve fault tolerance of observations; Among them, Λ ij is the triangular matrix obtained by Cholesky decomposition of the augmented error vector covariance matrix, is the noise vector of the double-difference GNSS observation after correction The covariance matrix of is the corrected UWB observation noise The variance of is the corrected position of aircraft i in the kth iteration.
4. The multi-aircraft collaborative positioning method based on robust message passing according to claim 3 is characterized in that: The weighting matrix is: Among them, Δ l is a vector The lth element, φ(Δ l ) is the function β(Δ l ), diag represents the diagonal elements of the matrix; Where η represents the scaling parameter, δp i represents the estimated error of the aircraft position, represents the combined noise vector of double-difference GNSS observations and UWB observations, Λ ij is the triangular matrix obtained by Cholesky decomposition of the augmented error vector covariance matrix.
5. The multi-aircraft cooperative positioning method based on robust message passing according to claim 1 is characterized in that: The double-difference GNSS observation likelihood function is: in, represents a Gaussian distribution, represents the double-difference GNSS observations between aircraft i and aircraft j and common-view satellites m and n, h DD (p i ,p j ) represents the mapping function from the positions of aircraft i and aircraft j to double-difference GNSS observations; h UWB (p i ,p j ) represents the mapping function from the positions of aircraft i and aircraft j to UWB observations, d ij represents the UWB ranging observation between adjacent aircraft i and aircraft j.
6. The multi-aircraft cooperative positioning method based on robust message passing according to claim 2, characterized in that: In the k+1th iteration, the aircraft's global edge posterior estimate of itself is: b k (p i )=f(p i ) Among them, f(p j ) represents the prior information of the position of aircraft i.
7. The multi-aircraft collaborative positioning method based on robust message passing according to claim 6 is characterized in that: When the iterations converge, each aircraft can obtain a posterior estimate of the edge position of the current positioning epoch, thus achieving distributed robust collaborative positioning.
8. The multi-aircraft cooperative positioning method based on robust message passing according to claim 7 is characterized in that: The iterative convergence is: the difference between the posterior estimates of the position edges of two adjacent positioning epochs is less than a set threshold.