Multi-vehicle cooperative positioning method based on progressive non-convex factor graph optimization

Abstract

The invention discloses a multi-vehicle cooperative positioning method based on progressive non-convex factor graph optimization, and the method comprises the steps: firstly calculating a GNSS pseudo-range error factor, an inter-vehicle distance measurement error factor and an inter-epoch constraint factor according to the collection data of a vehicle-mounted terminal GNSS sensor and a UWB distance measurement sensor; then a progressive non-convex welsch cost function is constructed, and an equivalent objective function is constructed in combination with a GNSS pseudo-range error factor, a workshop distance measurement error factor and an inter-epoch constraint factor; and finally, carrying out factor graph optimization on the equivalent objective function, solving an optimal weight and state, and outputting a global state and final weights of a GNSS pseudo-range error factor, a workshop distance measurement error factor and an inter-epoch constraint factor. According to the method, abnormal measurement values in cooperative positioning are suppressed by constructing a progressive non-convex welsch cost function, and meanwhile, multi-epoch and multi-node measurement information is fully fused by adopting a factor graph optimization architecture, so that the reliability of cooperative positioning is improved.

Description

Technical Field

[0001] The present invention relates to the field of satellite navigation technology, in particular to a multi-vehicle collaborative positioning method based on progressive non-convex factor graph optimization. Background Art

[0002] Accurate and reliable positioning systems are crucial for vehicle applications. Currently, Global Navigation Satellite Systems (GNSS) can provide continuous and accurate position information for vehicles in open environments. However, relying solely on GNSS in degraded urban environments can hinder the availability and accuracy of positioning. With the advancement of communication technology, collaborative positioning technology, based on inter-node communication and measurement, is gaining popularity. Compared to single-vehicle positioning, collaborative positioning benefits from information exchange between vehicles, achieving higher positioning accuracy and availability.

[0003] For collaborative positioning systems, ensuring their reliability is a key concern. Collaborative positioning systems face not only anomalies from GNSS measurements but also issues such as inter-node ranging anomalies. The larger the system and the greater the number of nodes, the higher the risk of anomalies. Existing methods for addressing anomalous measurements fall into two main categories: fault detection and exclusion (FDE) and robust estimation. The former typically uses statistical detection methods such as consistency checks to identify and exclude outliers, while the latter introduces robust cost functions to reduce the impact of anomalous measurements in the optimization algorithm. These methods are effective when suppressing a small number of anomalous measurements. However, in collaborative positioning systems, the probability of multiple anomalies occurring simultaneously increases, significantly increasing the risk of these methods reaching local optimality when faced with a large number of anomalies. When an FDE algorithm or a robust estimation algorithm reaches a local optimality, the final positioning result will exhibit unacceptable deviations. Summary of the Invention

[0004] The technical problem to be solved by the present invention is to provide a multi-vehicle collaborative positioning method based on progressively non-convex factor graph optimization. By constructing a progressively non-convex Welsch cost function, abnormal measurement values in collaborative positioning are suppressed. At the same time, a factor graph optimization architecture is adopted to fully integrate the measurement information of multiple epochs and multiple nodes, thereby improving the reliability of collaborative positioning.

[0005] The technical solution of the present invention is: A multi-vehicle collaborative localization method based on progressive non-convex factor graph optimization specifically includes the following steps: (1) Calculate the GNSS pseudorange error factor, vehicle-to-vehicle ranging error factor, and inter-epoch constraint factor based on the collected data from the vehicle-mounted GNSS sensor and the UWB ranging sensor; (2) Construct an asymptotically non-convex Welsch cost function. Based on the asymptotically non-convex Welsch cost function and the calculated GNSS pseudorange error factor, vehicle ranging error factor and inter-epoch constraint factor, construct the objective function of collaborative positioning. According to the Black-Rangarajan duality, the objective function is converted into an equivalent objective function. (3) Factor graph optimization is performed on the equivalent objective function to solve the optimal weights and states; the solving of the optimal weights and states specifically includes the following steps: parameter initialization, weight update, state update, update of non-convex control parameters and iteration termination, and finally outputs the global state and the final weights of the GNSS pseudorange error factor, the vehicle ranging error factor and the inter-epoch constraint factor.

[0006] The specific steps of calculating the GNSS pseudorange error factor, the vehicle ranging error factor, and the inter-epoch constraint factor based on the collected data of the vehicle-mounted GNSS sensor and the UWB ranging sensor are as follows: S11. Calculate the GNSS pseudorange error factor based on the GNSS pseudorange value output by the GNSS sensor, the GNSS ephemeris, and the differential information broadcast by the GNSS differential station. , see the following formula (1) for details: (1); In formula (1), is the GNSS pseudorange value, Differential information broadcast by GNSS differential stations, Represents the pseudorange observation model, and the satellite position is obtained from the GNSS ephemeris , and are the vehicle position and clock error to be solved respectively; S12. Calculate the inter-vehicle ranging error factor based on the inter-vehicle ranging value obtained by the UWB ranging sensor , see the following formula (2) for details: (2); In formula (2), is the inter-vehicle distance measurement value of the two vehicles at the current epoch, represents the workshop ranging model, and are the positions of the two vehicles participating in the inter-vehicle ranging at the current epoch; S13. Calculate the inter-epoch constraint factor based on the correlation between the time-domain differential carrier phase measurement value and the relative displacement between epochs. , see the following formula (3) for details: (3); In formula (3), , represents the relative displacement observation model between epochs, and The current epoch and the vehicle position to be solved in the previous epoch, is the relative displacement between epochs, which is measured by time-domain differential carrier phase The calculation is as follows (4): (4); In formula (4), represents the unit direction vector from the vehicle to the satellite, represents the receiver clock drift, It is the satellite's clock drift. represents the satellite displacement between epochs, Represents the satellite signal wavelength, represents the satellite clock drift between epochs, Represents the measurement residual of the differential carrier phase; The asymptotically non-convex Welsch cost function is expressed as follows (5): (5); In formula (5), represents the asymptotically non-convex Welsch cost function, represents the non-convexity control parameter, represents the shape parameter, The norm of the GNSS pseudorange error factor, vehicle-to-vehicle ranging error factor, or inter-epoch constraint factor.

[0007] The objective function of collaborative positioning is constructed based on the progressively non-convex Welsch cost function and the calculated GNSS pseudorange error factor, vehicle ranging error factor and epoch constraint factor. , the objective function is shown in the following formula (6): (6); In formula (6), is the global optimal state, the global state , Represents the total number of vehicles, is the transpose symbol, Representative The state set of the vehicle in the factor graph sliding window, Representative The vehicle is at the current epoch The state below, and are the vehicle's position and clock error, represents the starting epoch of the sliding window, is the length of the window; represents the set of all epochs within the factor graph sliding window, represents the set of all satellites, represents the set of all vehicles, represents the set of all vehicle distance measurements, is the GNSS pseudorange error factor The asymptotically non-convex Welsch cost function, is the vehicle-to-vehicle ranging error factor The asymptotically non-convex Welsch cost function, is based on the inter-epoch constraint factor The asymptotically non-convex Welsch cost function.

[0008] The process of converting the objective function into an equivalent objective function according to the Black-Rangarajan duality is specifically shown in the following formula (7): (7); In formula (7), is the global optimal state, is the optimal weight set, weight set Including 、 and , Represents the penalty item, for 、 or , 、 、 Represents the GNSS pseudorange error factors , vehicle ranging error factor and the norm of the inter-epoch constraint factor The weight of .

[0009] The factor graph optimization of the equivalent objective function to solve the optimal weights and states specifically includes the following steps: S31, parameter initialization: Initialize all values in the equivalent objective function to 1 and use the least squares method to solve the global initial state , and initialize the non-convexity control parameters to: ;in, Represents the global initial state The corresponding maximum residual, the maximum residual is the global initial state Under GNSS pseudorange error factor, vehicle ranging error factor and inter-epoch constraint factor, the maximum value; S32, weight update: In the In the iteration, using the state in the previous iteration Calculate pseudorange error factor , vehicle ranging error factor , inter-epoch constraint factor , and calculate the GNSS pseudorange error factor, vehicle ranging error factor and inter-epoch constraint factor norm according to the following formula (8) in the first The weight in the iteration; (8); In formula (8), Representative Non-convexity control parameter of the iteration; S33, Status Update: Substitute the weights calculated by formula (8) into the equivalent objective function and remove the penalty term to obtain The global state updated in the iteration , see the following formula (9) for details: (9); S34, update of non-convex control parameters, see the following formula (10): (10); In formula (10), is the rate of decrease of the non-convex control parameter, For the Non-convexity control parameter of the iteration; S35, iteration termination: When the non-convexity control parameter is less than 1 after the update, the iteration is terminated, and the global state obtained from the last iterative update, as well as the weights of the GNSS pseudorange error factor, the vehicle ranging error factor, and the inter-epoch constraint factor are output. The global state includes the position and clock error of all vehicles in the collaborative positioning system.

[0010] Advantages of the present invention: (1) The present invention constructs a progressively non-convex Welsch cost function and introduces a non-convexity control parameter to achieve variable non-convexity and adjustable robustness, thereby gradually changing the non-convexity of the cost function during the optimization process to alleviate the local optimal problem faced by non-convex optimization and effectively suppress abnormal observations in GNSS pseudorange and vehicle-to-vehicle ranging; (2) The present invention adopts a factor graph optimization architecture to fully integrate the measurement information of multiple epochs and multiple nodes, perform parameter initialization, weight update, state update, update of non-convexity control parameters and iteration termination, and reduce the influence of multipath and non-line-of-sight measurement values in GNSS and vehicle ranging by weighted method. The final output result is the status of each vehicle (i.e., node) in the collaborative positioning system, including the vehicle's position coordinates and clock error, thereby improving the reliability of collaborative positioning. BRIEF DESCRIPTION OF THE DRAWINGS

[0011] Figure 1 It is the overall architecture diagram of the present invention.

[0012] Figure 2 It is a flowchart of the factor graph optimization of the present invention.

[0013] Figure 3 is a linear graph of the asymptotically non-convex Welsch cost function of the present invention. DETAILED DESCRIPTION

[0014] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of the present invention.

[0015] See Figure 1 A multi-vehicle collaborative localization method based on progressive non-convex factor graph optimization specifically includes the following steps: (1) Based on the collected data from the vehicle-mounted GNSS sensor and the UWB (ultra-wideband) ranging sensor, the residual is calculated. The residual includes the GNSS pseudorange error factor, the vehicle ranging error factor, and the inter-epoch constraint factor. The specific steps are as follows: S11. Calculate the GNSS pseudorange error factor based on the GNSS pseudorange value output by the GNSS sensor, the GNSS ephemeris, and the differential information broadcast by the GNSS differential station. , see the following formula (1) for details: (1); In formula (1), is the GNSS pseudorange value, Differential information broadcast by GNSS differential stations, Represents the pseudorange observation model, and the satellite position is obtained from the GNSS ephemeris , and are the vehicle position and clock error to be solved respectively; S12. Calculate the inter-vehicle ranging error factor based on the inter-vehicle ranging value obtained by the UWB ranging sensor , see the following formula (2) for details: (2); In formula (2), is the inter-vehicle distance measurement value of the two vehicles at the current epoch, represents the workshop ranging model, and are the positions of the two vehicles participating in the inter-vehicle ranging at the current epoch; S13. Calculate the inter-epoch constraint factor based on the correlation between the time-domain differential carrier phase measurement value and the relative displacement between epochs. , see the following formula (3) for details: (3); In formula (3), , represents the relative displacement observation model between epochs, and The current epoch and the vehicle position to be solved in the previous epoch, is the relative displacement between epochs, which is measured by time-domain differential carrier phase The calculation is as follows (4): (4); In formula (4), represents the unit direction vector from the vehicle to the satellite, represents the receiver clock drift, It is the satellite's clock drift. represents the satellite displacement between epochs, Represents the satellite signal wavelength, represents the satellite clock drift between epochs, Represents the measurement residual of the differential carrier phase; (2) Construct an asymptotically non-convex Welsch cost function. Based on the asymptotically non-convex Welsch cost function and the calculated GNSS pseudorange error factor, vehicle ranging error factor and inter-epoch constraint factor, construct the objective function of collaborative positioning. According to the Black-Rangarajan duality, the objective function is converted into an equivalent objective function. The specific steps are as follows: S21. Construct asymptotically non-convex Welsch cost function, whose expression is shown below (5): (5); In formula (5), represents the asymptotically non-convex Welsch cost function, represents the non-convexity control parameter, represents the shape parameter, represents the norm of the GNSS pseudorange error factor, vehicle ranging error factor or inter-epoch constraint factor. The linear graph of the Welsch cost function is shown in Figure 3 ; S22. Constructing the objective function of collaborative positioning , the objective function is shown in the following formula (6): (6); In formula (6), is the global optimal state, the global state , Represents the total number of vehicles, is the transpose symbol, Representative The state set of the vehicle in the factor graph sliding window, Representative The vehicle is at the current epoch The state below, and are the vehicle's position and clock error, represents the starting epoch of the sliding window, is the length of the window; represents the set of all epochs within the factor graph sliding window, represents the set of all satellites, represents the set of all vehicles, represents the set of all vehicle distance measurements, is the GNSS pseudorange error factor The asymptotically non-convex Welsch cost function, is the vehicle-to-vehicle ranging error factor The asymptotically non-convex Welsch cost function, is based on the inter-epoch constraint factor The asymptotically non-convex Welsch cost function; S23. According to the Black-Rangarajan duality, the objective function is transformed into an equivalent objective function. That is, the process of Equation (6) is equivalently expressed using the Black-Rangarajan duality, which is equivalent to an iterative reweighted least squares problem, that is, the global state and weights are solved simultaneously, as shown in the following Equation (7): (7); In formula (7), is the global optimal state, is the optimal weight set, weight set Including 、 and , Represents the penalty item, for 、 or , 、 、 Represents the GNSS pseudorange error factors , vehicle ranging error factor and the norm of the inter-epoch constraint factor The weight of (3) Factor graph optimization of equivalent objective function (see Figure 2 ), solving the optimal weights and states, specifically including the following steps: S31, parameter initialization: Initialize all values in the equivalent objective function to 1 and use the least squares method to solve the global initial state , and initialize the non-convexity control parameters to: ;in, Represents the global initial state The corresponding maximum residual, the maximum residual is the global initial state Under GNSS pseudorange error factor, vehicle ranging error factor and inter-epoch constraint factor, the maximum value; S32, weight update: In the In the iteration, using the state in the previous iteration Calculate pseudorange error factor , vehicle ranging error factor , inter-epoch constraint factor , and calculate the GNSS pseudorange error factor, vehicle ranging error factor and inter-epoch constraint factor norm according to the following formula (8) in the first The weight in the iteration; (8); In formula (8), Representative Non-convexity control parameter of the iteration; S33, Status Update: Substitute the weights calculated by formula (8) into the equivalent objective function and remove the penalty term to obtain The global state updated in the iteration , see the following formula (9) for details: (9); S34, update of non-convex control parameters, see the following formula (10): (10); In formula (10), is the rate of decrease of the non-convex control parameter, For the Non-convexity control parameter of the iteration; S35, iteration termination: When the non-convexity control parameter is less than 1 after the update, the iteration is terminated, and the global state obtained from the last iterative update, as well as the weights of the GNSS pseudorange error factor, the vehicle ranging error factor, and the inter-epoch constraint factor are output. The global state includes the position and clock error of all vehicles in the collaborative positioning system.

[0016] While embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions, and variations may be made to these embodiments without departing from the principles and spirit of the invention, and that the scope of the invention is defined by the appended claims and their equivalents.

Claims

1. A multi-vehicle collaborative localization method based on progressive non-convex factor graph optimization, characterized by: The specific steps include: (1) Calculate the GNSS pseudorange error factor, vehicle-to-vehicle ranging error factor, and inter-epoch constraint factor based on the collected data from the vehicle-mounted GNSS sensor and the UWB ranging sensor; (2) Construct an asymptotically non-convex Welsch cost function. Based on the asymptotically non-convex Welsch cost function and the calculated GNSS pseudorange error factor, vehicle ranging error factor and inter-epoch constraint factor, construct the objective function of collaborative positioning. According to the Black-Rangarajan duality, the objective function is converted into an equivalent objective function. (3) Factor graph optimization is performed on the equivalent objective function to solve the optimal weights and states; the solving of the optimal weights and states specifically includes the following steps: parameter initialization, weight update, state update, update of non-convex control parameters and iteration termination, and finally outputs the global state and the final weights of the GNSS pseudorange error factor, the vehicle ranging error factor and the inter-epoch constraint factor.

2. The multi-vehicle collaborative localization method based on progressive non-convex factor graph optimization according to claim 1, characterized in that: The specific steps of calculating the GNSS pseudorange error factor, the vehicle ranging error factor, and the inter-epoch constraint factor based on the collected data of the vehicle-mounted GNSS sensor and the UWB ranging sensor are as follows: S11. Calculate the GNSS pseudorange error factor based on the GNSS pseudorange value output by the GNSS sensor, the GNSS ephemeris, and the differential information broadcast by the GNSS differential station. , see the following formula (1) for details: （1）； In formula (1), is the GNSS pseudorange value, Differential information broadcast by GNSS differential stations, Represents the pseudorange observation model, and the satellite position is obtained from the GNSS ephemeris , and are the vehicle position and clock error to be solved respectively; S12. Calculate the inter-vehicle ranging error factor based on the inter-vehicle ranging value obtained by the UWB ranging sensor , see the following formula (2) for details: （2）； In formula (2), is the inter-vehicle distance measurement value of the two vehicles at the current epoch, represents the workshop ranging model, and are the positions of the two vehicles participating in the inter-vehicle ranging at the current epoch; S13. Calculate the inter-epoch constraint factor based on the correlation between the time-domain differential carrier phase measurement value and the relative displacement between epochs. , see the following formula (3) for details: （3）； In formula (3), , represents the relative displacement observation model between epochs, and The current epoch and the vehicle position to be solved in the previous epoch, is the relative displacement between epochs, which is measured by time-domain differential carrier phase The calculation is as follows (4): （4）； In formula (4), represents the unit direction vector from the vehicle to the satellite, represents the receiver clock drift, It is the satellite's clock drift. represents the satellite displacement between epochs, Represents the satellite signal wavelength, represents the satellite clock drift between epochs, Represents the measurement residual of the differential carrier phase.

3. The multi-vehicle collaborative localization method based on progressive non-convex factor graph optimization according to claim 2, characterized in that: The asymptotically non-convex Welsch cost function is expressed as follows (5): （5）； In formula (5), represents the asymptotically non-convex Welsch cost function, represents the non-convexity control parameter, represents the shape parameter, The norm of the GNSS pseudorange error factor, vehicle-to-vehicle ranging error factor, or inter-epoch constraint factor.

4. The multi-vehicle collaborative localization method based on progressive non-convex factor graph optimization according to claim 3, characterized in that: The objective function of collaborative positioning is constructed based on the progressively non-convex Welsch cost function and the calculated GNSS pseudorange error factor, vehicle ranging error factor and inter-epoch constraint factor. , the objective function is shown in the following formula (6): （6）； In formula (6), is the global optimal state, the global state , Represents the total number of vehicles, is the transpose symbol, Representative The state set of the vehicle in the factor graph sliding window, Representative The vehicle is at the current epoch The state below, and are the vehicle's position and clock error, represents the starting epoch of the sliding window, is the length of the window; represents the set of all epochs within the factor graph sliding window, represents the set of all satellites, represents the set of all vehicles, represents the set of all vehicle distance measurements, is the GNSS pseudorange error factor The asymptotically non-convex Welsch cost function, is the vehicle-to-vehicle ranging error factor The asymptotically non-convex Welsch cost function, is based on the inter-epoch constraint factor The asymptotically non-convex Welsch cost function.

5. The multi-vehicle collaborative localization method based on progressive non-convex factor graph optimization according to claim 4, characterized in that: The process of converting the objective function into an equivalent objective function according to the Black-Rangarajan duality is specifically shown in the following formula (7): （7）； In formula (7), is the global optimal state, is the optimal weight set, weight set Including 、 and , Represents the penalty item, for 、 or , 、 、 Represents the GNSS pseudorange error factors , vehicle ranging error factor and the norm of the inter-epoch constraint factor The weight of .

6. The multi-vehicle collaborative localization method based on progressive non-convex factor graph optimization according to claim 4, characterized in that: The factor graph optimization of the equivalent objective function to solve the optimal weights and states specifically includes the following steps: S31, parameter initialization: Initialize all values in the equivalent objective function to 1 and use the least squares method to solve the global initial state , and initialize the non-convexity control parameters to: ;in, Represents the global initial state The corresponding maximum residual, the maximum residual is the global initial state Under GNSS pseudorange error factor, vehicle ranging error factor and inter-epoch constraint factor, the maximum value; S32, weight update: In the In the iteration, using the state in the previous iteration Calculate pseudorange error factor , vehicle ranging error factor , inter-epoch constraint factor , and calculate the GNSS pseudorange error factor, vehicle ranging error factor and inter-epoch constraint factor norm according to the following formula (8) in the first The weight in the iteration; （8）； In formula (8), Representative Non-convexity control parameter of the iteration; S33, Status Update: Substitute the weights calculated by formula (8) into the equivalent objective function and remove the penalty term to obtain The global state updated in the iteration , see the following formula (9) for details: （9）； S34, update of non-convex control parameters, see the following formula (10): （10）； In formula (10), is the rate of decrease of the non-convex control parameter, For the Non-convexity control parameter of the iteration; S35, iteration termination: When the non-convexity control parameter is less than 1 after the update, the iteration is terminated, and the global state obtained from the last iterative update, as well as the weights of the GNSS pseudorange error factor, the vehicle ranging error factor, and the inter-epoch constraint factor are output. The global state includes the position and clock error of all vehicles in the collaborative positioning system.

Images