A GNSS / UWB robust factor graph optimization cooperative positioning method for complex urban environment

Abstract

The application provides a GNSS / UWB robust factor graph optimization cooperative positioning method for urban complex environment, which is used in the high-precision cooperative positioning scene of vehicles in intelligent transportation and automatic driving. The method comprises the following steps: first, constructing a GNSS / UWB cooperative positioning factor graph model, fusing GNSS pseudorange double difference, inter-vehicle UWB ranging and epoch-to-epoch speed constraint; second, constructing a robust cost function, dividing the observation residual into normal domain, suspicious domain and gross error domain; third, designing a robust weight function based on progressive non-convex optimization and Black-Rangarajan dual relationship, realizing abnormal observation weight reduction or rejection; fourth, introducing an adaptive GNC strategy with fusion Hessian matrix positive definite nature callback mechanism, dynamically adjusting the control parameters. The method fuses robust optimization and adaptive non-convex adjustment, and significantly improves the positioning accuracy, reliability and real-time performance of cooperative vehicles in urban complex environment.

Description

Technical Field

[0001] This invention belongs to the field of intelligent transportation and high-precision vehicle positioning technology, specifically relating to a GNSS / UWB robust factor graph optimization cooperative positioning method for complex urban environments. Background Technology

[0002] Vehicle positioning, a crucial component of Intelligent Transportation Systems (ITS), plays a vital role in applications such as autonomous driving, lane-level navigation, and platooning. Accurate and reliable location information is fundamental to these applications. Global Navigation Satellite Systems (GNSS) are widely used for vehicle positioning due to their high precision, global coverage, and real-time performance. However, in complex urban environments, GNSS faces challenges such as reduced satellite count, non-line-of-sight (NLS) conditions, and severe multipath effects, hindering the provision of stable and reliable positioning results. With the rapid development of vehicular communication technology, Cooperative Positioning (CP), which involves sharing and fusing observation data from multiple vehicles, has become a major research direction. Compared to single-node positioning, CP methods can acquire more observational information from inter-vehicle communication, overcoming the limitations of individual observations and achieving higher positioning accuracy and availability.

[0003] CP systems incorporate multiple information sources, with relative ranging between nodes being a crucial component, typically acquired by radio ranging equipment. Ultra-wideband (UWB) technology, transmitting data via nanosecond-level non-sinusoidal narrow pulses, offers advantages such as high data transmission rates, low cost, and high ranging accuracy, providing efficient and precise ranging communication services for CP systems. Regarding CP algorithms, various state estimation algorithms, including Extended Kalman Filter (EKF), Belief Propagation (BP), and Factor Graph Optimization (FGO), are widely applied in CP systems. The FGO algorithm, in particular, reduces nonlinear errors through repeated linearization and reuses the temporal correlation between measurement data, demonstrating advantages in positioning accuracy. Zhuang C et al., from a geometric constraint perspective, fitted the road plane using the position information of cooperating vehicles, introducing plane constraints as an additional factor into the FGO framework, effectively mitigating the performance degradation caused by inter-vehicle ranging interruptions. Zhang G et al. proposed a 3D building model-assisted GNSS collaborative positioning (CP) method, utilizing ray tracing technology to correct NLOS and multipath errors. They also achieved multi-node joint optimization by fusing absolute positioning, relative positioning, and displacement constraints through free-form occlusion (FGO), achieving superior positioning accuracy compared to traditional GNSS double-difference methods in urban canyon scenarios. Hongbo Z et al. proposed an unmanned aerial vehicle (UAV)-assisted clustering optimization collaborative positioning strategy. By introducing UAVs as aerial relay nodes, they solved the problem of CP networks splitting into multiple isolated subclusters due to building occlusion in urban environments. However, most of these methods focus on increasing observation constraint information, lacking handling of outliers in observation data and insufficient consideration of FGO robustness. To improve the robustness of CP systems in complex urban environments, robust estimation strategies can be used to suppress the impact of outliers on state estimation. M-estimation, as a typical robust estimation method, uses a robust cost function instead of the quadratic cost in least squares to increase system robustness. This method has been widely used in both GNSS positioning and CP systems. However, most robust cost functions, such as the Hubre, Cauchy, and Geman-McClure functions, are non-convex. Directly optimizing their objective functions can lead to local optima, where normal observations are misclassified and discarded, while outliers are retained, causing the state estimation to converge in the wrong direction. To reduce the risk of getting trapped in local optima, the Graduate Non-Convexity (GNC) optimization method has been applied to CP systems.The GNC algorithm controls the non-convexity of the cost function by introducing control parameters. These parameters are updated iteratively, gradually restoring the cost function from a convex function back to its initial non-convex state, thus effectively reducing the risk of getting trapped in local optima while ensuring system robustness. Chitturi S et al. proposed an adaptive GNC strategy based on the characteristics of the graph Laplacian spectrum in the field of image averaging, which effectively improved the balance between efficiency and accuracy in solving large-scale problems, but this method has not yet been introduced into CP systems. Wang J et al. replaced the traditional least squares cost in FGO with the Welsch cost function and avoided local optima through GNC, enabling the CP system to correctly reduce the weight of outliers even with a high proportion of outliers, significantly improving the robustness of the CP system. Zhao H et al. introduced the truncated least squares cost function into the FGO cooperative localization framework and used the GNC strategy to overcome the local optima problem caused by the non-convexity of the truncated least squares cost function, achieving effective suppression of outlier observations in the GNSS / UWB multi-source fusion CP system. However, both of the above methods use a fixed-factor GNC method, that is, gradually decreasing the control parameters according to a preset ratio, which makes it impossible to achieve a balance between accuracy and computational efficiency.

[0004] To address the issue of CP systems being severely compromised in positioning accuracy and reliability due to measurement outliers in urban environments, this invention proposes a robust factor graph collaborative positioning method for GNSS / UWB in complex urban environments. First, a robust cost function is constructed, dividing the residuals into three intervals: normal, suspicious, and gross errors. Different penalty strategies are applied to different intervals to reduce the weighting of outliers during the Free-Oriented Optimization (FGO) process. Based on this, the robust cost function is integrated into the GNC framework, and the corresponding robust weight function is derived based on the Black-Rangarajan duality, transforming the robust optimization problem into an alternating iteration of weighted least squares and weight function updates, effectively reducing the local optima problem when directly optimizing non-convex costs. Finally, an adaptive GNC strategy incorporating a Hessian matrix positive definiteness callback mechanism is constructed based on the residual distribution characteristics to dynamically adjust control parameters, accelerating the convergence process while maintaining optimization stability. Summary of the Invention

[0005] To address the issues of insufficient robustness of traditional factor graph optimization methods and the tendency to get trapped in local optima when directly optimizing non-convex robust cost functions in complex urban environments, this invention proposes a GNSS / UWB robust factor graph optimization cooperative positioning method for complex urban environments. This method theoretically achieves a deep integration of multi-source observation fusion, robust estimation, and adaptive optimization, ensuring high-precision and reliable positioning of cooperative vehicles in complex urban environments.

[0006] A robust factor map optimization cooperative localization method for complex urban environments includes the following steps:

[0007] Step 1: Construct a GNSS / UWB cooperative positioning factor graph model. Take the three-dimensional positions of multiple epochs and multiple vehicle nodes within the sliding window as the global state to be estimated. Based on GNSS pseudorange double difference, UWB ranging between vehicles and adjacent epoch velocity constraints, construct an error function and form a cooperative positioning nonlinear least squares objective function.

[0008] Step 2: Construct a robust cost function. Based on the standardized residual distribution, divide the observations into the normal domain, the questionable domain, and the gross error domain. Retain, reduce, or remove the observations respectively. Introduce control parameters to adjust the non-convexity of the cost function, providing a robust cost function for subsequent asymptotic non-convex optimization.

[0009] Step 3: Construct robust weight function through Black-Rangarajan duality to transform robust factor graph optimization into joint optimization of state variables and observation weights; set initial observation weights, obtain initial state based on conventional FGO, and complete anomaly trigger judgment and control parameter initialization based on initial residuals; update observation weights when the state is fixed, and update vehicle node state when the weights are fixed to achieve anomaly observation weight reduction or removal.

[0010] Step 4: Propose an adaptive GNC strategy that integrates the positive definiteness callback mechanism of the Hessian matrix. Generate candidate parameters based on the percentile of the residual critical control parameters, and verify them using Hessian positive definiteness or Cholesky decomposition. Dynamically accept or call back control parameters until the final collaborative localization result is output.

[0011] Step 1 describes the construction of a GNSS / UWB cooperative positioning factor graph model, which takes the three-dimensional positions of multiple epochs and multiple vehicle nodes within the sliding window as the global state to be estimated. An error function is constructed based on GNSS pseudorange double difference, UWB ranging between vehicles, and adjacent epoch velocity constraints, and a cooperative positioning nonlinear least squares objective function is formed.

[0012] The specific steps are as follows:

[0013] Step 1-1: In FGO, combine the estimated states from multiple epochs within the sliding window to form a global state vector. Defined as:

[0014] (1)

[0015] In the formula, Represents the current epoch. Represents the length of the sliding window. represent The state to be estimated in an epoch is defined as:

[0016] (2)

[0017] In the formula, Assign vehicle node number. Indicates the vehicle node at the current epoch. The three-dimensional position in the geocentric coordinate system.

[0018] Steps 1-2: Construct GNSS pseudorange double difference factor, vehicle node In the calendar The GNSS pseudorange observation equation can be expressed as:

[0019] (3)

[0020] In the formula, Indicates satellite At frequency Above and vehicle nodes pseudorange observations between This represents the actual geometric distance between the node and the satellite. Indicates receiver clock bias. Indicates satellite clock bias, Indicates tropospheric delay, Indicates ionospheric delay, This indicates other errors caused by non-line-of-sight, multipath, and pseudorange noise.

[0021] Introduction of reference station The vehicle node, i.e., the mobile station, is obtained. With the base station Pseudorange double-difference observation equation:

[0022] (4)

[0023] In the formula, This represents double difference operation. and Representing the reference satellite and non-reference satellite. Double-difference operation can eliminate receiver clock bias and satellite clock bias. Since the GNSS signal propagation paths from the satellite to the rover and to the base station are similar, double-difference processing can significantly reduce the effects of ionospheric and tropospheric delays. When the distance between the rover and the base station is less than 10 kilometers, the double-difference ionospheric and tropospheric delays can be ignored. Therefore, in the case of short baselines, the pseudorange double-difference observation equation can be simplified as:

[0024] (5)

[0025] The vehicle node can be obtained from equation (5). With base station The double-difference pseudorange error function is:

[0026] (6)

[0027] In the formula, , It indicates the three-dimensional position of the reference satellite and non-reference satellite in the geocentric coordinate system, which can be obtained through broadcast ephemeris.

[0028] In a cooperative vehicle network, each vehicle can also form a pseudorange double-difference observation equation with other vehicles. From equations (5) and (6), the vehicle node can be derived. With vehicle nodes The pseudorange error function between them is:

[0029] (7)

[0030] It should be noted that in complex environments such as urban canyons, pseudorange observations are easily affected by severe non-line-of-sight delays and multipath effects, leading to... The error increases significantly, and double-difference processing cannot eliminate the impact of this type of error.

[0031] Steps 1-3: In the cooperative vehicle network, UWB ranging factors are constructed by obtaining the distances between vehicles using UWB. For nodes... and nodes The UWB ranging observation equation between them is:

[0032] (8)

[0033] In the formula, The distance between vehicles is obtained through UWB measurement. It is the actual distance between vehicles. The ranging error is caused by non-line-of-sight, multipath effects, and UWB noise.

[0034] The vehicle node can be derived from equation (8). and vehicle nodes The distance measurement error function between them is:

[0035] (9)

[0036] When there are obstructions between vehicles, UWB ranging is severely affected by non-line-of-sight and multipath effects, leading to errors in the error term. It will increase significantly.

[0037] To establish the temporal correlation between multiple epochs in the factor graph, GNSS velocity measurement results are introduced to constrain the vehicle node positions of adjacent epochs, forming an inter-epoch velocity constraint factor. For vehicle nodes... The observation equations can be listed as follows:

[0038] (10)

[0039] In the formula, The velocity measurement results are from GNSS Doppler observations. Let be the time interval between adjacent epochs. From equation (10), the velocity error function can be derived as:

[0040] (11)

[0041] Combining all the above error functions, the objective function of vehicle cooperative localization based on FGO can be expressed as a nonlinear least squares optimization:

[0042] (12)

[0043] In the formula, The optimal estimate representing the global state. Represents the set of all satellites viewed from the same location. Represents the set of all vehicle nodes. and This represents the covariance matrix of the double-difference pseudorange observations between vehicle nodes and base stations, and between vehicle nodes. The covariance matrix representing the distance observations between vehicle nodes. The covariance matrix represents the velocity constraint between epochs.

[0044] Step 2 describes the construction of a robust cost function. Based on the standardized residual distribution, the observations are divided into normal domain, doubtful domain, and gross error domain. The observations are retained, weighted down, or removed respectively. Control parameters are introduced to adjust the non-convexity of the cost function, providing a robust cost function for subsequent asymptotic non-convex optimization.

[0045] The specific steps are as follows:

[0046] Step 2-1: In complex urban environments, measurement residuals exhibit different trends depending on the observation conditions: when observation conditions are good, the residual distribution is close to the noise level; as observation conditions worsen, the residuals also increase. In some cases, excessive errors due to non-line-of-sight and multipath effects lead to severe residual deterioration. A single form of robust cost function is usually difficult to simultaneously consider the retention of weights in small residual intervals, the smooth reduction of weights in medium residual intervals, and the strong suppression requirements in large residual intervals. To solve this problem, this paper divides the residual domain into three intervals: normal domain, suspicious domain, and gross error domain. Different residual processing strategies are adopted for each interval to form a robust cost function. At the same time, as a non-convex function, directly optimizing the non-convex cost carries the risk of getting trapped in local optima. GNC reduces the risk of getting trapped in local optima by introducing a control parameter to gradually restore the cost function from a convex function back to the original non-convex cost. Replacing the quadratic cost function in equation (12) with the robust cost function that introduces the control parameter, we get:

[0047] (13)

[0048] Step 2-2 This represents the robust cost function for adjustable nonconvexity after adding control parameters. (Adjustable nonconvexity robust cost function) and robust cost function The representation is as follows:

[0049] (14)

[0050] (15)

[0051] In the formula, Represents the standardized residual. , There are two judgment thresholds, which are generally set based on experience. , , To adjust the control parameters for non-convexity, the standardized residuals can be calculated using the following formula:

[0052] (16)

[0053] In the formula, This represents the residuals calculated using all error functions. This is the mean square error of all current residuals. , This represents taking the median value. This is the normalization coefficient for the absolute deviation of the median.

[0054] Step 3 describes constructing a robust weight function using the Black-Rangarajan duality relation, transforming robust factor graph optimization into a joint optimization of state variables and observation weights; setting initial observation weights, obtaining the initial state based on conventional FGO, and completing anomaly trigger judgment and control parameter initialization based on the initial residuals; updating observation weights when the state is fixed, and updating vehicle node states when the weights are fixed, thereby achieving anomaly observation weight reduction or removal.

[0055] The specific steps are as follows:

[0056] Step 3-1: It is difficult to optimize equation (13) by directly solving the objective function. Therefore, we can use the Black-Rangarajan duality to transform the robust optimization problem in equation (13) into:

[0057] (17)

[0058] In the formula, , , and These represent the weights corresponding to the double-difference pseudorange measurement between the vehicle and the base station, the double-difference pseudorange measurement between vehicles, the UWB ranging measurement, and the speed constraint between epochs, respectively. All weights range from 0 to 1. Represents a set composed of all weights. This represents the optimal solution for all weights. The penalty function for the weights is expressed as a function whose form depends on the cost function and can be derived from the Black-Rangarajan duality.

[0059] Step 3-2: In the initial stage, set the weight of all observations to 1. The set representing all initial weights is obtained by performing a nonlinear least squares solution of FGO before adaptive GNC calculation. That is, the initial global state, through Calculate the standardized initial residuals for all factors. .

[0060] To avoid introducing unnecessary robust iterations when observation quality is good, this paper sets a basis for adaptive GNC optimization before optimization. Anomaly triggering criteria for thresholds. Based on a comprehensive consideration of the overall residual distribution and local anomalies, the threshold is determined as follows: Sort by size from smallest to largest, and take the 95th percentile. With the maximum residual respectively with , Comparison:

[0061] (18)

[0062] If any of the conditions in the formula are met, it is considered that there are anomalous observations in the current epoch that require robust handling, and the adaptive GNC optimization process begins; otherwise, it is considered that the vast majority of observations are still in the process of optimization. Within the quadratic interval, its weight remains at 1, and the result of the regular FGO solution is directly used as the output of the current epoch.

[0063] The control parameters for the nonconvexity of the control cost function are initialized as follows: To ensure that all residuals in the initial stage lie in the convex part of the robust cost function, the following must be satisfied:

[0064] (19)

[0065] In the formula, This represents the percentile of the factor residuals. Therefore, the initial control parameters... Possible options:

[0066] (20)

[0067] At this point, all residuals are located in the convex interval, and the objective function is approximately a convex function, which is beneficial for obtaining a stable initial convergence direction, after which adaptive GNC solution is performed.

[0068] Step 3-3, in the... In the next adaptive GNC iteration, the global state obtained from the previous round of estimation is fixed. Control parameters The weights of each factor are updated by minimizing the part of the objective function related to the weight variables shown in equation (17). Since the weight variables of each factor are independent of each other, the global weight update problem can be decomposed into several independent one-dimensional optimization problems. Solving all factor residuals The objective function corresponding to the current iteration weights can be expressed as:

[0069] (twenty one)

[0070] According to the Black-Rangarajan duality, the optimal weights satisfy:

[0071] (twenty two)

[0072] In the formula, The first derivative is:

[0073] (twenty three)

[0074] The optimal weights for the current iteration, i.e., the robust weight function, can be expressed as:

[0075] (twenty four)

[0076] Will Substituting the values ​​into the equation yields the optimal weight for the current iteration.

[0077] After the weight update is complete, obtain , , and That is, the weights of all observations of various factors, and global state updates are performed under the condition that the weights are fixed. The objective function is:

[0078] (25)

[0079] Due to weighting penalty It depends only on the weights and is independent of the global state, so it can be removed from the objective function during the state update phase. At this point, the robust optimization problem is transformed into a weighted nonlinear least squares optimization problem.

[0080] Step 4 proposes an adaptive GNC strategy that integrates the positive definiteness callback mechanism of the Hessian matrix. Candidate parameters are generated based on the percentile of the residual critical control parameters, and tested using Hessian positive definiteness or Cholesky decomposition. Control parameters are dynamically accepted or called back until the final cooperative localization result is output.

[0081] The specific steps are as follows:

[0082] Step 4-1: After the state update is completed, the control parameters need to be adjusted according to the current residual distribution. Adaptive updates, the core of adaptive GNC, lies in how to ensure convergence from the optimized state to the global optimum. Reduce to 1 as quickly as possible to restore the original state. .

[0083] In the After the next state update is completed, the current global state will be used as a basis. Recalculate the standardized residuals of all factors, and define the critical control parameter for any factor residual to enter the reduced-weight interval as follows:

[0084] (26)

[0085] Construct a set of critical control parameters corresponding to all factors:

[0086] (27)

[0087] In the formula, This represents the set of residuals for all factors. This paper does not use a reduction factor to directly reduce the set. Instead, based on the set percentiles Determine candidate control parameters. Let Represents a set The Percentile, then the th The candidate control parameters for the next iteration are defined as follows:

[0088] (28)

[0089] Step 4-2, Percentiles This is used to control the proportion of residuals allowed to enter the reduced-weight interval in each iteration. In the initial stage, let... at this time:

[0090] (29)

[0091] The result of the above equation is consistent with the definition of the initialization phase. The results are the same; in actual calculations, initialization... The calculation is performed using equation (20).

[0092] To avoid excessive non-convexity of the objective function due to a rapid decrease in control parameters, this paper constructs the Hessian matrix under the current candidate control parameters and performs a positive definiteness test to determine whether the robust objective function maintains a stable local second-order approximation near the current estimate. The Hessian matrix is ​​the local second-order curvature matrix of the robust objective function with respect to the state variables, and its form is as follows:

[0093] (30)

[0094] In the formula, The Jacobian matrix for all factors. For based on The weight matrix is ​​composed of second derivatives. The current candidate control parameters correspond to... The second derivative can be expressed as:

[0095] (31)

[0096] Whether the objective function remains locally positive definite around the current estimated value under the current control parameters is determined by the Hessian matrix. The representation is as follows:

[0097] (32)

[0098] In the formula, The threshold for positive definiteness is defined as follows: if the above formula is satisfied, it indicates that the objective function maintains local positive definiteness near the current state under the candidate control parameters, and the degree of non-convexity of the current objective function is considered acceptable. In actual calculations, equivalence can be determined through Cholesky decomposition.

[0099] (33)

[0100] If the Cholesky decomposition is successful, the Hessian matrix is ​​considered to satisfy the positive definiteness requirement. In this case, candidate control parameters are accepted, and the percentiles are further reduced in the next iteration. :

[0101] (34)

[0102] (35)

[0103] In the formula, Percentiles The minimum value of , in this article Its significance lies in avoiding the simultaneous deweighting of more than half of the observation factors in the same stage, thus preserving the constraint effect of most observations on state estimation, reducing the risk of non-positive definiteness of the Hessian matrix, and improving the stability of the adaptive GNC process. The step size is used to control the rate at which the non-convexity of the objective function is enhanced. In this paper... .

[0104] Step 4-3: When the Hessian matrix does not satisfy the positive definiteness condition or the Cholesky decomposition fails, i.e.:

[0105] (36)

[0106] when If the value is too small, the non-convexity of the objective function becomes too strong, which may lead to the optimization getting stuck in a local optimum. In this case, the percentile should be adjusted back to increase the candidate control parameter.

[0107] (37)

[0108] In the formula, This is the upper bound of the percentiles accepted in the previous iteration. To back off the step size, in this paper After the callback, the calculation is recalculated using equation (30). And perform the Hessian matrix positive definiteness test again until it passes. The calculated Hessian matrix satisfies the positive definiteness requirement.

[0109] when When the value equals 1, the original robust cost function is used for weight and state updates. The optimization is considered complete, and the final estimation result is output.

[0110] Beneficial effects of this invention:

[0111] 1. To improve the observation and utilization capabilities of vehicle cooperative positioning in complex urban environments, this invention constructs a GNSS / UWB cooperative positioning factor graph model, which integrates GNSS pseudorange double difference, UWB ranging between vehicles, and adjacent epoch velocity constraints, making full use of multi-vehicle and multi-epoch observation information to effectively improve the accuracy and availability of cooperative positioning.

[0112] 2. To address the positioning result deviations caused by GNSS pseudorange anomalies, UWB ranging errors, and multipath effects, this invention proposes a robust cost function that divides the standardized residuals into normal, questionable, and gross error domains. Different observations are retained, smoothed down, or eliminated respectively, effectively suppressing the impact of abnormal observations on state estimation.

[0113] 3. To reduce the risk of getting trapped in local optima when directly optimizing non-convex robust cost functions, this invention constructs robust weight functions based on the Black-Rangarajan duality, transforming robust factor graph optimization into joint optimization of state variables and observation weights. By iteratively updating weights and states, the stability and solvability of the robust optimization process are improved.

[0114] 4. To balance positioning accuracy and computational efficiency, this invention proposes an adaptive GNC strategy that integrates the positive definiteness callback mechanism of the Hessian matrix. The control parameters are dynamically adjusted according to the residual distribution, and the degree of non-convexity is controlled by checking the positive definiteness of the Hessian matrix or Cholesky decomposition. While ensuring optimization stability, the number of iterations is reduced, effectively improving the real-time performance and reliability of collaborative positioning in complex urban environments. Attached Figure Description

[0115] Figure 1 is a flowchart of a GNSS / UWB robust factor graph optimization cooperative localization method for complex urban environments according to the present invention;

[0116] Figure 2 is a detailed flowchart of step 1 of one embodiment of the present invention;

[0117] Figure 3 is a detailed flowchart of step 2 of one embodiment of the present invention;

[0118] Figure 4 is a detailed flowchart of step 3 of one embodiment of the present invention;

[0119] Figure 5 is a detailed flowchart of step 4 of one embodiment of the present invention;

[0120] Figure 6 is a summary flowchart of one embodiment of the present invention;

[0121] Figure 7 is a diagram of the experimental equipment of the present invention;

[0122] Figure 8 The experimental vehicle of this invention and Satellite count map;

[0123] Figure 9 Add satellite random error plot to this invention

[0124] Figure 10 This is a diagram showing the distances between vehicles in the experiment of this invention;

[0125] Figure 11 This is a trajectory diagram of vehicle 1 in the experiment of this invention;

[0126] Figure 12 This is a diagram showing the position error of vehicle 1 in the experiment of this invention;

[0127] Figure 13 This is a graph showing the cumulative distribution function of two-dimensional positioning errors under different methods of the present invention;

[0128] Figure 14 The calculation time is for different methods of the present invention. Detailed Implementation

[0129] An embodiment of the present invention will be further described below with reference to the accompanying drawings.

[0130] In this embodiment of the invention, a robust factor graph optimization cooperative localization method for GNSS / UWB in complex urban environments is provided, such as... Figure 1 As shown, it includes the following steps:

[0131] Step 1: Construct a GNSS / UWB cooperative positioning factor graph model. Take the three-dimensional positions of multiple epochs and multiple vehicle nodes within the sliding window as the global state to be estimated. Based on GNSS pseudorange double difference, UWB ranging between vehicles and adjacent epoch velocity constraints, construct an error function and form a cooperative positioning nonlinear least squares objective function.

[0132] Step 1-1: In FGO, combine the estimated states from multiple epochs within the sliding window to form a global state vector. Defined as:

[0133] (1)

[0134] In the formula, Represents the current epoch. Represents the length of the sliding window. represent The state to be estimated in an epoch is defined as:

[0135] (2)

[0136] In the formula, Assign vehicle node number. Indicates the vehicle node at the current epoch. The three-dimensional position in the geocentric coordinate system.

[0137] Steps 1-2: Construct GNSS pseudorange double difference factor, vehicle node In the calendar The GNSS pseudorange observation equation can be expressed as:

[0138] (3)

[0139] In the formula, Indicates satellite At frequency Above and vehicle nodes pseudorange observations between This represents the actual geometric distance between the node and the satellite. Indicates receiver clock bias. Indicates satellite clock bias, Indicates tropospheric delay, Indicates ionospheric delay, This indicates other errors caused by non-line-of-sight, multipath, and pseudorange noise.

[0140] Introduction of reference station The vehicle node, i.e., the mobile station, is obtained. With the base station Pseudorange double-difference observation equation:

[0141] (4)

[0142] In the formula, This represents double difference operation. and Representing the reference satellite and non-reference satellite. Double-difference operation can eliminate receiver clock bias and satellite clock bias. Since the GNSS signal propagation paths from the satellite to the rover and to the base station are similar, double-difference processing can significantly reduce the effects of ionospheric and tropospheric delays. When the distance between the rover and the base station is less than 10 kilometers, the double-difference ionospheric and tropospheric delays can be ignored. Therefore, in the case of short baselines, the pseudorange double-difference observation equation can be simplified as:

[0143] (5)

[0144] The vehicle node can be obtained from equation (5). With base station The double-difference pseudorange error function is:

[0145] (6)

[0146] In the formula, , It indicates the three-dimensional position of the reference satellite and non-reference satellite in the geocentric coordinate system, which can be obtained through broadcast ephemeris.

[0147] In a cooperative vehicle network, each vehicle can also form a pseudorange double-difference observation equation with other vehicles. From equations (5) and (6), the vehicle node can be derived. With vehicle nodes The pseudorange error function between them is:

[0148] (7)

[0149] It should be noted that in complex environments such as urban canyons, pseudorange observations are easily affected by severe non-line-of-sight delays and multipath effects, leading to... The error increases significantly, and double-difference processing cannot eliminate the impact of this type of error.

[0150] Steps 1-3: In the cooperative vehicle network, UWB ranging factors are constructed by obtaining the distances between vehicles using UWB. For nodes... and nodes The UWB ranging observation equation between them is:

[0151] (8)

[0152] In the formula, The distance between vehicles is obtained through UWB measurement. It is the actual distance between the vehicles. The ranging error is caused by non-line-of-sight, multipath effects, and UWB noise.

[0153] The vehicle node can be derived from equation (8). and vehicle nodes The distance measurement error function between them is:

[0154] (9)

[0155] When there are obstructions between vehicles, UWB ranging is severely affected by non-line-of-sight and multipath effects, leading to errors in the error term. It will increase significantly.

[0156] To establish the temporal correlation between multiple epochs in the factor graph, GNSS velocity measurement results are introduced to constrain the vehicle node positions of adjacent epochs, forming an inter-epoch velocity constraint factor. For vehicle nodes... The observation equations can be listed as follows:

[0157] (10)

[0158] In the formula, The velocity measurement results are from GNSS Doppler observations. Let be the time interval between adjacent epochs. From equation (10), the velocity error function can be derived as:

[0159] (11)

[0160] Combining all the above error functions, the objective function of vehicle cooperative localization based on FGO can be expressed as a nonlinear least squares optimization:

[0161] (12)

[0162] In the formula, The optimal estimate representing the global state. Represents the set of all satellites viewed from the same location. Represents the set of all vehicle nodes. and This represents the covariance matrix of the double-difference pseudorange observations between vehicle nodes and base stations, and between vehicle nodes. The covariance matrix representing the distance observations between vehicle nodes. The covariance matrix represents the velocity constraint between epochs.

[0163] Step 2: Construct a robust cost function. Based on the standardized residual distribution, divide the observations into the normal domain, the questionable domain, and the gross error domain. Retain, reduce, or remove the observations respectively. Introduce control parameters to adjust the non-convexity of the cost function, providing a robust cost function for subsequent asymptotic non-convex optimization.

[0164] Step 2-1: In complex urban environments, measurement residuals exhibit different trends depending on the observation conditions: when observation conditions are good, the residual distribution is close to the noise level; as observation conditions worsen, the residuals also increase. In some cases, excessive errors due to non-line-of-sight and multipath effects lead to severe residual deterioration. A single form of robust cost function is usually difficult to simultaneously consider the retention of weights in small residual intervals, the smooth reduction of weights in medium residual intervals, and the strong suppression requirements in large residual intervals. To solve this problem, this paper divides the residual domain into three intervals: normal domain, suspicious domain, and gross error domain. Different residual processing strategies are adopted for each interval to form a robust cost function. At the same time, as a non-convex function, directly optimizing the non-convex cost carries the risk of getting trapped in local optima. GNC reduces the risk of getting trapped in local optima by introducing a control parameter to gradually restore the cost function from a convex function back to the original non-convex cost. Replacing the quadratic cost function in equation (12) with the robust cost function that introduces the control parameter, we get:

[0165] (13)

[0166] Step 2-2 This represents the robust cost function for adjustable nonconvexity after adding control parameters. (Adjustable nonconvexity robust cost function) and robust cost function The representation is as follows:

[0167] (14)

[0168] (15)

[0169] In the formula, Represents the standardized residual. , There are two judgment thresholds, which are generally set based on experience. , , To adjust the control parameters for non-convexity, the standardized residuals can be calculated using the following formula:

[0170] (16)

[0171] In the formula, This represents the residuals calculated using all error functions. This is the mean square error of all current residuals. , This represents taking the median value. This is the normalization coefficient for the absolute deviation of the median.

[0172] Step 3: Construct robust weight function through Black-Rangarajan duality to transform robust factor graph optimization into joint optimization of state variables and observation weights; set initial observation weights, obtain initial state based on conventional FGO, and complete anomaly trigger judgment and control parameter initialization based on initial residuals; update observation weights when the state is fixed, and update vehicle node state when the weights are fixed to achieve anomaly observation weight reduction or removal.

[0173] Step 3-1: It is difficult to optimize equation (13) by directly solving the objective function. Therefore, we can use the Black-Rangarajan duality to transform the robust optimization problem in equation (13) into:

[0174] (17)

[0175] In the formula, , , and These represent the weights corresponding to the double-difference pseudorange measurement between the vehicle and the base station, the double-difference pseudorange measurement between vehicles, the UWB ranging measurement, and the speed constraint between epochs, respectively. All weights range from 0 to 1. Represents a set composed of all weights. This represents the optimal solution for all weights. The penalty function for the weights is expressed as a function whose form depends on the cost function and can be derived from the Black-Rangarajan duality.

[0176] Step 3-2: In the initial stage, set the weight of all observations to 1. The set representing all initial weights is obtained by performing a nonlinear least squares solution of FGO before adaptive GNC calculation. That is, the initial global state, through Calculate the standardized initial residuals for all factors. .

[0177] To avoid introducing unnecessary robust iterations when observation quality is good, this paper sets a basis for adaptive GNC optimization before optimization. Anomaly triggering criteria for thresholds. Based on a comprehensive consideration of the overall residual distribution and local anomalies, the threshold is determined as follows: Sort by size from smallest to largest, and take the 95th percentile. With the maximum residual respectively with , Comparison:

[0178] (18)

[0179] If any of the conditions in the formula are met, it is considered that there are anomalous observations in the current epoch that require robust handling, and the adaptive GNC optimization process begins; otherwise, it is considered that the vast majority of observations are still in the process of optimization. Within the quadratic interval, its weight remains at 1, and the result of the regular FGO solution is directly used as the output of the current epoch.

[0180] The control parameters for the nonconvexity of the control cost function are initialized as follows: To ensure that all residuals in the initial stage lie in the convex part of the robust cost function, the following must be satisfied:

[0181] (19)

[0182] In the formula, This represents the percentile of the factor residuals. Therefore, the initial control parameters... Possible options:

[0183] (20)

[0184] At this point, all residuals are located in the convex interval, and the objective function is approximately a convex function, which is beneficial for obtaining a stable initial convergence direction, after which adaptive GNC solution is performed.

[0185] Step 3-3, in the... In the next adaptive GNC iteration, the global state obtained from the previous round of estimation is fixed. Control parameters The weights of each factor are updated by minimizing the part of the objective function related to the weight variables shown in equation (17). Since the weight variables of each factor are independent of each other, the global weight update problem can be decomposed into several independent one-dimensional optimization problems. Solving all factor residuals The objective function corresponding to the current iteration weights can be expressed as:

[0186] (twenty one)

[0187] According to the Black-Rangarajan duality, the optimal weights satisfy:

[0188] (twenty two)

[0189] In the formula, The first derivative is:

[0190] (twenty three)

[0191] The optimal weights for the current iteration, i.e., the robust weight function, can be expressed as:

[0192] (twenty four)

[0193] Will Substituting the values ​​into the equation yields the optimal weight for the current iteration.

[0194] After the weight update is complete, obtain , , and That is, the weights of all observations of various factors, and global state updates are performed under the condition that the weights are fixed. The objective function is:

[0195] (25)

[0196] Due to weighting penalty It depends only on the weights and is independent of the global state, so it can be removed from the objective function during the state update phase. At this point, the robust optimization problem is transformed into a weighted nonlinear least squares optimization problem.

[0197] Step 4: Propose an adaptive GNC strategy that integrates the positive definiteness callback mechanism of the Hessian matrix. Generate candidate parameters based on the percentile of the residual critical control parameters, and verify them using Hessian positive definiteness or Cholesky decomposition. Dynamically accept or call back control parameters until the final collaborative localization result is output.

[0198] Step 4-1: After the state update is completed, the control parameters need to be adjusted according to the current residual distribution. Adaptive updates, the core of adaptive GNC, lies in how to ensure convergence from the optimized state to the global optimum. Reduce to 1 as quickly as possible to restore the original state. .

[0199] In the After the next state update is completed, the current global state will be used as a basis. Recalculate the standardized residuals of all factors, and define the critical control parameter for any factor residual to enter the reduced-weight interval as follows:

[0200] (26)

[0201] Construct a set of critical control parameters corresponding to all factors:

[0202] (27)

[0203] In the formula, This represents the set of residuals for all factors. This paper does not use a reduction factor to directly reduce the set. Instead, based on the set percentiles Determine candidate control parameters. Let Represents a set The Percentile, then the th The candidate control parameters for the next iteration are defined as follows:

[0204] (28)

[0205] Step 4-2, Percentiles This is used to control the proportion of residuals allowed to enter the reduced-weight interval in each iteration. In the initial stage, let... at this time:

[0206] (29)

[0207] The result of the above equation is consistent with the definition of the initialization phase. The results are the same; in actual calculations, initialization... The calculation is performed using equation (20).

[0208] To avoid excessive non-convexity of the objective function due to a rapid decrease in control parameters, this paper constructs the Hessian matrix under the current candidate control parameters and performs a positive definiteness test to determine whether the robust objective function maintains a stable local second-order approximation near the current estimate. The Hessian matrix is ​​the local second-order curvature matrix of the robust objective function with respect to the state variables, and its form is as follows:

[0209] (30)

[0210] In the formula, The Jacobian matrix for all factors. Based on The weight matrix is ​​composed of second derivatives. The current candidate control parameters correspond to... The second derivative can be expressed as:

[0211] (31)

[0212] Whether the objective function remains locally positive definite around the current estimated value under the current control parameters is determined by the Hessian matrix. The representation is as follows:

[0213] (32)

[0214] In the formula, The threshold for positive definiteness is defined as follows: if the above formula is satisfied, it indicates that the objective function maintains local positive definiteness near the current state under the candidate control parameters, and the degree of non-convexity of the current objective function is considered acceptable. In actual calculations, equivalence can be determined through Cholesky decomposition.

[0215] (33)

[0216] If the Cholesky decomposition is successful, the Hessian matrix is ​​considered to satisfy the positive definiteness requirement. In this case, candidate control parameters are accepted, and the percentiles are further reduced in the next iteration. :

[0217] (34)

[0218] (35)

[0219] In the formula, Percentiles The minimum value of , in this article Its significance lies in avoiding the simultaneous deweighting of more than half of the observation factors in the same stage, thus preserving the constraint effect of most observations on state estimation, reducing the risk of non-positive definiteness of the Hessian matrix, and improving the stability of the adaptive GNC process. The step size is used to control the rate at which the non-convexity of the objective function is enhanced. In this paper... .

[0220] Step 4-3: When the Hessian matrix does not satisfy the positive definiteness condition or the Cholesky decomposition fails, i.e.:

[0221] (36)

[0222] when If the value is too small, the non-convexity of the objective function becomes too strong, which may lead to the optimization getting stuck in a local optimum. In this case, the percentile should be adjusted back to increase the candidate control parameter.

[0223] (37)

[0224] In the formula, This is the upper bound of the percentiles accepted in the previous iteration. To back off the step size, in this paper After the callback, the calculation is recalculated using equation (30). And perform the Hessian matrix positive definiteness test again until it passes. The calculated Hessian matrix satisfies the positive definiteness requirement.

[0225] when When the value equals 1, the original robust cost function is used for weight and state updates. The optimization is considered complete, and the final estimation result is output.

[0226] like Figure 7 As shown, to verify the practical performance of the GNSS / UWB robust factor graph optimization cooperative localization method for complex urban environments proposed in this invention, a vehicle cooperative localization experiment was designed. Two vehicles participated in the experiment, named... and All vehicles travel along the same route, and both vehicles always remain within the effective distance of UWB ranging. Equipped with a PD09 GNSS receiver, a LinkTrack PB UWB module, and an MP-POS510 GNSS / INS combined positioning and attitude determination system, It is equipped with a PD09 GNSS receiver, a LinkTrack PB UWB module, and an Apollo RTK / INS integrated positioning module. The PD09 GNSS receiver collects raw GNSS observation data, including GPS L1 / L2 and BDS B1 / B2 observations. The LinkTrack PB UWB module collects vehicle-to-vehicle distance measurements. The MP-POS510 GNSS / INS integrated positioning and attitude determination system and the Apollo RTK / INS integrated positioning module provide centimeter-level positioning accuracy. The results serve as... and The reference trajectory.

[0227] The sampling frequency of the GNSS receiver in each vehicle is 1Hz. At the same time, the GNSS receiver provides timing for the UWB, and the sampling frequency of the UWB is also set to 1Hz. In order to eliminate the influence of the boom arm between GNSS and UWB, the phase centers of the two antennas are located on the same vertical line. and Visible satellite images Figure 8 As shown, the experiment's data acquisition time was 324 seconds. To simulate the possibility of significant errors in GNSS pseudorange observations under complex urban environments, random errors were added to the pseudorange observations of satellites G07, G08, G09, C07, C10, and C12 during the epoch 29–299. Figure 9 As shown.

[0228] from Figure 9 As can be seen, different degrees of random error were added to the pseudorange observations of different satellites, with the error range between ±20 meters. Satellites G07, G08, G09, C07, C10, and C12 were selected because these six satellites were used by vehicles during data acquisition. , The satellites shared the same view with the base station, and there was no interruption in observation.

[0229] Because the two vehicles were close together and there were no obstructions during data acquisition, the UWB ranging values ​​were less affected by NLOS and multipath errors. To verify the robustness of the method, ranging errors such as [missing information] were added during the time period from epoch 29 to 299. Figure 10 As shown.

[0230] To verify the localization performance and practical effects of the proposed method, four cooperative localization schemes were designed:

[0231] (1) FGO: The factor graph optimization co-localization method without robust estimation has an objective function as shown in Equation (11) and adopts a quadratic cost function;

[0232] (2) FGO-RCF: A factor graph optimization co-localization method implemented with robust cost function. The adaptive GNC process is removed in the method in this paper so as to distinguish between the presence and absence of GNC;

[0233] (3) FGO-GNC-Welsch: A robust estimation co-localization method based on FGO and GNC-Welsch. This method replaces the quadratic cost of the objective function with the Welsch cost based on GNC, thereby enhancing the robustness of co-localization.

[0234] (4) FGO-GNCp-RCF: This paper proposes an FGO co-localization method that integrates robust cost function and adaptive GNC.

[0235] In urban environments, roads are typically flat; therefore, this paper primarily focuses on planar positioning performance, i.e., two-dimensional position error. Furthermore, since all vehicles exhibit similar positioning performance trends under different schemes, this will be the focus of subsequent analysis. The location results. Figure 11 Vehicles on display in China The trajectory of its movement.

[0236] Depend on Figure 11 It is evident that the traditional FGO method, employing a quadratic cost function, is highly sensitive to outliers, exhibiting the worst robustness and showing significant deviations in the calculated trajectory. While the FGO-RCF method delivers a better overall trajectory than the traditional FGO, its direct optimization of a non-convex cost function carries the risk of getting trapped in local optima, resulting in substantial deviations in some trajectories. In contrast, FGO-GNC-Welsch and FGO-GNCp-RCF demonstrate superior performance in trajectory continuity and overall consistency, effectively suppressing the impact of outlier observations and exhibiting stronger robustness.

[0237] To better compare the positioning accuracy of the FGO, FGO-RCF, FGO-GNC-Welsch, and FGO-GNCp-RCF methods, the positioning errors of the four methods are statistically analyzed, such as... Figure 12 As shown.

[0238] Depend on Figure 12It is evident that, without adding a robust cost function, the traditional FGO is highly sensitive to outliers and exhibits the worst localization accuracy. Compared to the FGO scheme, other schemes demonstrate varying degrees of robustness. Compared to the traditional FGO method, FGO-RCF has a certain ability to suppress outliers and can improve the accuracy of the localization results to some extent. However, due to the non-convexity of the RCF cost function itself, its direct application to the FGO optimization process can easily lead to the objective function getting trapped in local optima at epochs with large initial value deviations or strong influence from outliers. This can result in increased localization errors in some epochs. In terms of localization accuracy, FGO-RCF reduces the RMSE from 8.69 m to 5.01 m compared to FGO, but the MAX increases from 21.79 m to 33.12 m.

[0239] Further comparison of the FGO-GNC-Welsch and FGO-GNCp-RCF methods reveals that the robust FGO method achieves improved localization accuracy after incorporating the GNC strategy. Compared to the FGO-RCF method, which directly employs RCF, the GNC strategy mitigates the local optima problem caused by the non-convex objective function, resulting in more stable anomaly suppression. Among the two robust GNC methods, FGO-GNCp-RCF outperforms FGO-GNC-Welsch in localization performance. The Welsch cost function uses a continuous exponential weighting method, smoothly reducing the observation weights as the residuals increase, thereby weakening the impact of observations with larger residuals. However, because its weight changes lack a clear elimination interval and are sensitive to residual scale parameters, some anomaly observations may still be assigned high weights when absorbed by state estimation during optimization and appear as smaller residuals. RCF, on the other hand, divides observations into three intervals—normal, suspicious, and gross—based on residual size, more clearly distinguishing between normal and anomaly observations; when some residuals exceed a threshold, the observation weight is adjusted to 0, and the observation is directly eliminated.

[0240] With the assistance of the adaptive GNC strategy, FGO-GNCp-RCF not only alleviated the local optimum problem caused by non-convex optimization, but also made full use of the strong robustness of RCF to anomaly observation, thus obtaining the best localization results. The two-dimensional root mean square error was reduced to 0.45 m, an improvement of approximately 95% compared to the FGO scheme, approximately 91% compared to the FGO-RCF scheme, and approximately 12% compared to the FGO-GNC-Welsch scheme. Figure 13 The text demonstrates different methods. The cumulative distribution function of two-dimensional positioning error.

[0241] This paper introduces an adaptive control mechanism based on the GNC algorithm. To verify the improvement in time efficiency of this method, the computation time of the proposed method and other control group methods are statistically analyzed and compared. All data processing was performed on a laptop equipped with a 12th Gen Intel(R) Core(TM) i5-12500H processor and 16 GB of RAM, with the sliding window length set to 2.

[0242] Figure 14 This section demonstrates the computation time per epoch for four methods in continuous data processing. Among these methods, the standard FGO method has the lowest computational burden, shortest processing time, and most stable performance because it does not include any robust estimation or outlier removal process and requires no additional weight calculations or iterations. Compared to the FGO method, the FGO-RCF method, due to the introduction of a robust cost function, requires iterative optimization using an iterative weighted least squares method in a single state estimation, thus increasing its processing time.

[0243] For both the FGO-GNC-Welsch and the proposed FGO-GNCp-RCF methods, while the GNC mechanism significantly improves positioning accuracy, its outer iterations inevitably increase the computational burden. However, as shown in the figure, the processing time of the proposed FGO-GNCp-RCF method is shorter than that of the FGO-GNC-Welsch method. This is because the GNC method relies on a fixed reduction factor, resulting in a lengthy number of iterations, while FGO-GNCp-RCF, through its adaptive control parameter strategy, can rapidly decrease or revert control parameters based on the current residual distribution, thus reducing the number of iterations.

[0244] The total observation data was 324 s, with both GNSS and UWB sampling frequencies at 1 Hz. The total runtime of the FGO-GNC-Welsch algorithm was 76.86 s, with an average single-epoch time of approximately 0.24 s. The FGO-GNCp-RCF algorithm, through adaptive GNC, reduced the total runtime to 52.90 s, with an average single-epoch solution time of 0.16 s. The proposed method, while meeting the real-time positioning requirements, improves the single-epoch solution efficiency by approximately 31.17% compared to the FGO-GNC-Welsch algorithm.

[0245] The above description is merely the most basic specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any substitutions that can be understood by those skilled in the art within the technical scope disclosed in the present invention should be included within the scope of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. A robust factor graph optimization cooperative localization method for complex urban environments using GNSS / UWB, characterized in that, Includes the following steps: Step 1: Construct a GNSS / UWB cooperative positioning factor graph model. Take the three-dimensional positions of multiple epochs and multiple vehicle nodes within the sliding window as the global state to be estimated. Based on GNSS pseudorange double difference, UWB ranging between vehicles and adjacent epoch velocity constraints, construct an error function and form a cooperative positioning nonlinear least squares objective function. Step 2: Construct a robust cost function. Based on the standardized residual distribution, divide the observations into the normal domain, the questionable domain, and the gross error domain. Retain, reduce, or remove the observations respectively. Introduce control parameters to adjust the non-convexity of the cost function, providing a robust cost function for subsequent asymptotic non-convex optimization. Step 3: Construct robust weight function through Black-Rangarajan duality to transform robust factor graph optimization into joint optimization of state variables and observation weights; set initial observation weights, obtain initial state based on conventional FGO, and complete anomaly trigger judgment and control parameter initialization based on initial residuals; update observation weights when the state is fixed, and update vehicle node state when the weights are fixed to achieve anomaly observation weight reduction or removal. Step 4: Propose an adaptive GNC strategy that integrates the positive definiteness callback mechanism of the Hessian matrix. Generate candidate parameters based on the percentile of the residual critical control parameters, and verify them using Hessian positive definiteness or Cholesky decomposition. Dynamically accept or call back control parameters until the final collaborative localization result is output. Step 1 describes the construction of a GNSS / UWB cooperative positioning factor graph model, which takes the three-dimensional positions of multiple epochs and multiple vehicle nodes within the sliding window as the global state to be estimated. An error function is constructed based on GNSS pseudorange double difference, UWB ranging between vehicles, and adjacent epoch velocity constraints, and a cooperative positioning nonlinear least squares objective function is formed. The specific steps are as follows: Step 1-1: In FGO, combine the estimated states from multiple epochs within the sliding window to form a global state vector. Defined as: （1） In the formula, Represents the current epoch. Represents the length of the sliding window. represent The state to be estimated in an epoch is defined as: （2） In the formula, Assign vehicle node number. Indicates the vehicle node at the current epoch. Three-dimensional position in the geocentric-fixed coordinate system; Steps 1-2: Construct GNSS pseudorange double difference factor, vehicle node In the calendar The GNSS pseudorange observation equation can be expressed as: （3） In the formula, Indicates satellite At frequency Above and vehicle nodes pseudorange observations between This represents the actual geometric distance between the node and the satellite. Indicates receiver clock bias. Indicates satellite clock bias, Indicates tropospheric delay, Indicates ionospheric delay, This indicates other errors caused by non-line-of-sight, multipath, and pseudorange noise; Introduction of reference station The vehicle node, i.e., the mobile station, is obtained. With the base station Pseudorange double-difference observation equation: （4） In the formula, This represents double difference operation. and Representing reference and non-reference satellites; double-difference operation can eliminate receiver clock bias and satellite clock bias; since the GNSS signal propagation paths from the satellite to the rover and to the base station are similar, double-difference processing can significantly reduce the effects of ionospheric and tropospheric delays; when the distance between the rover and the base station is less than 10 kilometers, the double-difference ionospheric and tropospheric delays can be ignored; therefore, in the case of short baselines, the pseudorange double-difference observation equation can be simplified as: （5） The vehicle node can be obtained from equation (5). With base station The double-difference pseudorange error function is: （6） In the formula, , The three-dimensional positions of reference and non-reference satellites in the geocentric and geofixed coordinate system can be obtained through broadcast ephemeris. In a cooperative vehicle network, each vehicle can also form a pseudorange double-difference observation equation with other vehicles. From equations (5) and (6), the vehicle node can be derived. With vehicle nodes The pseudorange error function between them is: （7） It should be noted that in complex environments such as urban canyons, pseudorange observations are easily affected by severe non-line-of-sight delays and multipath effects, leading to... The error increases significantly, and double-difference processing cannot eliminate the impact of this type of error. Steps 1-3: In the cooperative vehicle network, the distance between vehicles is obtained through UWB to construct the UWB ranging factor; for nodes... and nodes The UWB ranging observation equation between them is: （8） In the formula, The distance between vehicles is obtained through UWB measurement. It is the actual distance between the vehicles. The ranging error is caused by non-line-of-sight, multipath effects, and UWB noise. The vehicle node can be derived from equation (8). and vehicle nodes The distance measurement error function between them is: （9） When there are obstructions between vehicles, UWB ranging is severely affected by non-line-of-sight and multipath effects, leading to errors in the error term. It will increase significantly; To establish the temporal correlation between multiple epochs in the factor graph, GNSS velocity measurement results are introduced to constrain the vehicle node positions of adjacent epochs, forming an inter-epoch velocity constraint factor; for vehicle nodes... The observation equations can be listed as follows: （10） In the formula, The velocity measurement results are from GNSS Doppler observations. The time interval between adjacent epochs; from equation (10), the velocity error function can be derived as: （11） Combining all the above error functions, the objective function of vehicle cooperative localization based on FGO can be expressed as a nonlinear least squares optimization: （12） In the formula, The optimal estimate representing the global state. Represents the set of all satellites viewed from the same location. Represents the set of all vehicle nodes. and This represents the covariance matrix of the double-difference pseudorange observations between vehicle nodes and base stations, and between vehicle nodes. The covariance matrix representing the distance observations between vehicle nodes. The covariance matrix representing the velocity constraints between epochs; Step 2 describes the construction of a robust cost function. Based on the standardized residual distribution, the observations are divided into normal domain, doubtful domain, and gross error domain. The observations are retained, weighted down, or removed respectively. Control parameters are introduced to adjust the non-convexity of the cost function, providing a robust cost function for subsequent asymptotic non-convex optimization. The specific steps are as follows: Step 2-1: In complex urban environments, measurement residuals exhibit different trends as observation conditions change: when observation conditions are good, the residual distribution is close to the noise level; as observation conditions deteriorate, the residuals also increase. In some cases, due to excessive errors caused by non-line-of-sight and multipath effects, the residuals are severely deteriorated. A single form of robust cost function is usually difficult to simultaneously take into account the retention of weights in small residual intervals, the smooth reduction of weights in medium residual intervals, and the strong suppression requirements in large residual intervals. To solve this problem, this paper divides the residual domain into three intervals: normal domain, suspicious domain, and gross error domain. Different residual processing strategies are adopted for each interval to form a robust cost function. At the same time, as a non-convex function, directly optimizing the non-convex cost carries the risk of getting trapped in local optima. GNC reduces the risk of getting trapped in local optima by introducing a control parameter to gradually restore the cost function from a convex function back to the original non-convex cost. The robust cost function with the introduced control parameter replaces the quadratic cost function in equation (12) to obtain: （13） Step 2-2 The robust cost function representing the adjustable nonconvexity after adding control parameters; the robust cost function representing the adjustable nonconvexity. and robust cost function The representation is as follows: （14） （15） In the formula, Represents the standardized residual. , There are two judgment thresholds, which are generally set based on experience. , , To adjust the control parameters for non-convexity, the standardized residuals can be calculated using the following formula: （16） In the formula, This represents the residuals calculated using all error functions. This is the mean square error of all current residuals. , This represents taking the median value. This is the normalization coefficient for the absolute deviation of the median.

2. The GNSS / UWB robust factor graph optimization cooperative localization method for complex urban environments according to claim 1, characterized in that, Step 3 describes constructing a robust weight function using the Black-Rangarajan duality relation, transforming robust factor graph optimization into a joint optimization of state variables and observation weights; setting initial observation weights, obtaining the initial state based on conventional FGO, and completing anomaly trigger judgment and control parameter initialization based on the initial residuals; updating observation weights when the state is fixed, and updating vehicle node states when the weights are fixed, thereby achieving anomaly observation weight reduction or removal. The specific steps are as follows: Step 3-1: It is difficult to optimize equation (13) by directly solving the objective function. Therefore, we can use the Black-Rangarajan duality to transform the robust optimization problem in equation (13) into: （17） In the formula, , , and These represent the weights corresponding to the double-difference pseudorange measurement between the vehicle and the base station, the double-difference pseudorange measurement between vehicles, the UWB ranging measurement, and the speed constraint between epochs, respectively. All weights range from 0 to 1. Represents a set composed of all weights. Represents the optimal solution for all weights; The penalty function for the weights, whose form depends on the cost function, can be derived from the Black-Rangarajan duality; Step 3-2: In the initial stage, set the weight of all observations to 1. The set representing all initial weights is obtained by performing a nonlinear least squares solution of FGO before adaptive GNC calculation. That is, the initial global state, through Calculate the standardized initial residuals for all factors. ; To avoid introducing unnecessary robust iterations when observation quality is good, this paper sets a basis for adaptive GNC optimization before optimization. Abnormal triggering criteria for thresholds; based on a comprehensive consideration of the overall residual distribution and local anomalies, for Sort by size from smallest to largest, and take the 95th percentile. With the maximum residual respectively with , Comparison: （18） When any of the conditions in the formula are met, it is considered that there are abnormal observations in the current epoch that require robust handling, and the adaptive GNC optimization process is entered. Otherwise, it is assumed that the vast majority of observations are still in the process of being conducted. Within the quadratic interval, its weight remains 1, and the regular FGO solution result is directly used as the output of the current epoch; The control parameters for the nonconvexity of the control cost function are initialized as follows: To ensure that all residuals in the initial stage lie in the convex part of the robust cost function, the following must be satisfied: （19） In the formula, This represents the percentile of the factor residuals; therefore, the initial control parameters... Possible options: （20） At this point, all residuals are located in the convex interval, and the objective function is approximately a convex function, which is beneficial for obtaining a stable initial convergence direction, and then adaptive GNC solution is performed. Step 3-3, in the... In the next adaptive GNC iteration, the global state obtained from the previous round of estimation is fixed. Control parameters By minimizing the part of the objective function shown in equation (17) that is related to the weight variables, the weights of each factor are updated; since the weight variables of each factor are independent of each other, the global weight update problem can be decomposed into several independent one-dimensional optimization problems; Solving all factor residuals The objective function corresponding to the current iteration weights can be expressed as: （21） According to the Black-Rangarajan duality, the optimal weights satisfy: （22） In the formula, The first derivative is: （23） The optimal weights for the current iteration, i.e., the robust weight function, can be expressed as: （24） Will Substituting the values ​​into the equation yields the optimal weight for the current iteration. After the weight update is complete, obtain , , and That is, the weights of all observations of various factors, and global state updates are performed under the condition that the weights are fixed. The objective function is: （25） Due to weighting penalty It depends only on the weights and is independent of the global state, so it can be removed from the objective function during the state update phase; at this point, the robust optimization problem is transformed into a weighted nonlinear least squares optimization problem.

3. The GNSS / UWB robust factor graph optimization cooperative positioning method for complex urban environments according to claim 1, characterized in that, Step 4 describes the proposed adaptive GNC strategy that integrates the positive definiteness callback mechanism of the Hessian matrix. Candidate parameters are generated based on the percentile of the residual critical control parameters, and tested using Hessian positive definiteness or Cholesky decomposition. Control parameters are dynamically accepted or called back until the final collaborative localization result is output. The specific steps are as follows: Step 4-1: After the state update is completed, the control parameters need to be adjusted according to the current residual distribution. Adaptive updates, the core of adaptive GNC, lies in how to ensure convergence from the optimized state to the global optimum. Reduce to 1 as quickly as possible to restore the original state. ; In the After the next state update is completed, the current global state will be used as a basis. Recalculate the standardized residuals of all factors, and define the critical control parameter for any factor residual to enter the reduced-weight interval as follows: （26） Construct a set of critical control parameters corresponding to all factors: （27） In the formula, This represents the set of residuals for all factors; this paper does not use a reduction factor to directly reduce the set. Instead, based on the set percentiles Determine candidate control parameters; let Represents a set The Percentile, then the th The candidate control parameters for the next iteration are defined as follows: （28） Step 4-2, Percentiles Used to control the proportion of residuals allowed to enter the reduced-weight interval in each iteration; In the initial stage, let at this time: （29） The result of the above equation is consistent with the definition of the initialization phase. The results are the same; in actual calculations, initialization... Calculate using equation (20); To avoid excessive non-convexity of the objective function due to a rapid decrease in control parameters, this paper constructs the Hessian matrix under the current candidate control parameters and performs a positive definiteness test to determine whether the robust objective function maintains a stable local second-order approximation near the current estimate. The Hessian matrix is ​​the local second-order curvature matrix of the robust objective function with respect to the state variables, and its form is as follows: （30） In the formula, The Jacobian matrix for all factors. Based on The weight matrix is ​​composed of second derivatives; the current candidate control parameters correspond to... The second derivative can be expressed as: （31） Whether the objective function remains locally positive definite around the current estimated value under the current control parameters is determined by the Hessian matrix. The representation is as follows: （32） In the formula, The threshold for positive definiteness is defined as follows: if the above formula is satisfied, it indicates that the objective function maintains local positive definiteness near the current state under the candidate control parameters, and the degree of non-convexity of the current objective function is considered acceptable. In actual calculations, equivalent judgment can be made through Cholesky decomposition. （33） If the Cholesky decomposition is successful, the Hessian matrix is ​​considered to satisfy the positive definiteness requirement. In this case, candidate control parameters are accepted, and the percentiles are further reduced in the next iteration. : （34） （35） In the formula, Percentiles The minimum value of , in this article Its significance lies in avoiding the simultaneous deweighting of more than half of the observation factors in the same stage, thus preserving the constraint effect of most observations on state estimation, reducing the risk of non-positive definiteness of the Hessian matrix, and improving the stability of the adaptive GNC process. The step size is used to control the rate at which the non-convexity of the objective function is enhanced. In this paper... ; Step 4-3: When the Hessian matrix does not satisfy the positive definiteness condition or the Cholesky decomposition fails, i.e.: （36） when If the value is too small, the non-convexity of the objective function is too strong, which may lead to the optimization getting stuck in a local optimum. In this case, the percentile should be adjusted to increase the candidate control parameter. （37） In the formula, This is the upper bound of the percentiles accepted in the previous iteration. To back off the step size, in this paper After the callback, the calculation is recalculated using equation (30). And perform the Hessian matrix positive definiteness test again until it passes. The calculated Hessian matrix satisfies the positive definiteness requirement; when When the value equals 1, the original robust cost function is used for weight and state updates. The optimization is considered complete, and the final estimation result is output.

Images