Method for suppressing outliers by using a progressive non-convex robust function for collaborative positioning

Abstract

The present invention belongs to the field of satellite navigation and discloses a method for collaborative positioning by suppressing outliers using a progressive non-convex robust function. First, raw data is obtained from the sensors carried by the vehicle and, after processing, is expressed in the form of residuals of each observable quantity. Secondly, in the factor graph algorithm framework, a truncated least squares (TLS) function assisted by a progressive non-convex method is used to robustly estimate GNSS pseudorange double differences, GNSS carrier double differences, and inter-vehicle ranging values, and the vehicle state and the weights of each observable quantity are alternately solved. Finally, after multiple iterations, when the weights corresponding to each observable quantity all become 0 or 1, the iteration is stopped, and the estimated result of the vehicle state at this time is output as the final positioning solution of the vehicle. The present invention can detect faulty observables without additional equipment, and the amount of computation is significantly reduced compared with existing methods, with a wider application range and higher usability.

Description

Technical Field

[0001] The present invention belongs to the field of satellite navigation, and particularly relates to a method for suppressing outliers and collaborative positioning by using a progressive non-convex robust function. Background Art

[0002] Collaborative positioning is a positioning technology based on vehicle ad-hoc networks that has emerged with the rise of the Internet of Vehicles. When vehicles perform collaborative positioning, they can not only utilize local measurement data but also measurement data shared by collaborative vehicles. Compared with the positioning method of a single vehicle with multiple sensors, the collaborative positioning method has the following advantages: the measurement values from collaborative nodes can improve the geometric dilution of precision and enhance the positioning accuracy; additional measurement values can increase the dimension of observations and the number of positioning equations, improving the positioning availability; most collaborative positioning methods do not require expensive devices such as lidar, mainly using vehicle-mounted communication and low-cost ranging units, resulting in very low costs; it does not involve complex image processing and data matching, occupying less vehicle-mounted resources and ensuring the real-time nature of positioning. Existing collaborative positioning methods are all implemented based on vehicle-to-vehicle measurements, and currently, there is no collaborative positioning method that can be achieved without vehicle-to-vehicle measurements. However, due to factors such as safety, privacy, permissions, and equipment costs, not all collaborative vehicles have the ability to provide raw measurement information, and the vehicle to be positioned may not be able to obtain the measurement data of collaborative vehicles. Additionally, if there is a large delay in the transmission of collaborative measurement data, the measurement data cannot be used for the positioning solution at the current moment.

[0003] M-estimation is an estimation method in robust estimation, used to more reliably estimate parameters in the presence of outliers or noisy data. Traditional least squares estimation is highly sensitive to outliers, while M-estimation suppresses the influence of outliers by introducing different loss functions, thereby improving the robustness of the estimation. In collaborative positioning, it is necessary to obtain raw GNSS observation data and ranging data between nodes. However, usually, due to various interferences, the observed values may deviate significantly. Therefore, compared with the least squares method, M-estimation allows the use of different loss functions (such as Huber, Cauchy, and Geman-McClure functions) to control the influence of the residuals of these faulty observed values, reducing the impact of outliers on the overall positioning result by giving these observed values less weight.

[0004] Different from the measurement domain collaborative positioning method, the positioning domain collaborative positioning method only uses the position data of collaborative vehicles to achieve the positioning solution. Therefore, it is of great practical significance to propose a positioning domain collaborative positioning method that weights each observed value by using a progressive non-convex truncated least squares function, thereby improving the availability and robustness of vehicle positioning in urban environments. Summary of the Invention

[0005] The object of the present invention is to weight the observation data of the sensors carried by a vehicle by using a progressive non-convex truncated least squares function, so as to improve the accuracy and robustness of vehicle positioning in an urban environment. In a dense urban environment, the occlusion of high-rise buildings, bridges and even trees will lead to the degradation of satellite observations or vehicle-to-vehicle ranging. In addition, the probability of non-line-of-sight propagation or multipath effects of signals increases significantly. These faults will degrade the overall positioning accuracy of the vehicle and it is difficult to meet the positioning accuracy requirements of high-level vehicle applications.

[0006] The present invention proposes a method for suppressing outliers and collaborative positioning by using a progressive non-convex robust function to solve the problem of positioning degradation caused by the occlusion of objects such as high-rise buildings in an urban environment. The implementation steps are as follows:

[0007] S1. Obtain the original GNSS observation data and vehicle-to-vehicle ranging information carried by the vehicle, and convert this information into a form related to the residuals of each observation.

[0008] S2. Use the factor graph algorithm to fuse the residual data of each observation from different sensors and at different times.

[0009] S3. Select a truncated least squares function assisted by a progressive non-convex method as the robust function in the M-estimation, and weight the vehicle GNSS carrier double-difference observations, GNSS pseudorange double-difference observations and vehicle-to-vehicle ranging observations, and iterate continuously to solve the optimal state of the vehicle and the optimal weights of each observation in turn.

[0010] S4. When, in the iteration, the weights corresponding to each observation finally become 0 or 1, stop the iteration and output the positioning result at this time as the final vehicle position.

[0011] Among them, the "converting this information into a form related to the residuals of each observation" mentioned in S1 includes GNSS pseudorange double-difference residuals, GNSS carrier double-difference residuals, GNSS Doppler velocity residuals and ultra-wideband (UWB) vehicle-to-vehicle ranging residuals. The specific method is as follows:

[0012] S11. Obtain the original GNSS data of the vehicle and the original GNSS data of the reference base station, and extract and save the pseudorange, elevation angle, carrier, Doppler and carrier-to-noise ratio information.

[0013] S12. Obtain and save the vehicle-to-vehicle ranging information obtained by the ultra-wideband (UWB) sensor at the current moment and the position information of the cooperative vehicle obtained through the vehicle network, and at the same time convert the vehicle position information into coordinates in the ECEF coordinate system.

[0014] S13. Calculate the accurate positions of each GNSS satellite in the ECEF coordinate system at this time according to the ephemeris file and save them.

[0015] S14. Select the satellite position information at the current moment and previous moments, the GNSS raw data information saved in S11, and the workshop ranging information saved in S13 according to the window length set by the factor graph algorithm. Eliminate all satellites with too small elevation angles in the GNSS raw information at each moment, and select the satellite that is common to the base station and the vehicle and has the largest carrier-to-noise ratio as the double-difference reference satellite;

[0016] S15. According to the information at different moments screened in S14, first use the GNSS pseudorange to calculate the positions of the vehicle at different moments, and then use the Doppler data to calculate the vehicle speeds at different moments;

[0017] S16. Set the vehicle positions at different moments calculated in S15 as the initial values for vehicle state estimation in subsequent iterations, and calculate the pseudorange double-difference residuals, carrier double-difference residuals, Doppler velocity residuals, and ultra-wideband vehicle ranging residuals respectively by combining the information screened in S14.

[0018] The specific process of S2 is as follows:

[0019] The factor graph algorithm fuses the GNSS pseudorange double-difference, GNSS carrier double-difference, GNSS velocity, and ultra-wideband workshop ranging information at the current moment and multiple previous moments. The specific solution equation is as follows:

[0020]

[0021] Among them, χ * is the optimal state of the vehicle; and e m,dv,t respectively represent the GNSS pseudorange double-difference residual, GNSS carrier double-difference residual, ultra-wideband workshop ranging residual, and GNSS velocity residual factors at time t; and respectively represent the weights related to the GNSS pseudorange and carrier observables and the weight of the workshop ranging observation value at time t.

[0022] The specific steps of S3 are as follows:

[0023] S31. Before iteration, first set the weights of all observables to 1, and solve for the vehicle state according to the solution equation of S2;

[0024] S32. Calculate the GNSS pseudorange double-difference residual and the workshop ranging value residual according to the vehicle position calculated in the previous step. The specific calculation formulas are as follows:

[0025]

[0026] Among them, the weight is expressed as θ, and the weights of GNSS observation quantities and vehicle-to-vehicle ranging observation quantities should be calculated according to the non-convex interval parameter c of their corresponding control robustness functions and the non-convexity parameter β of the control functions, respectively;

[0027] If it is the first iteration, the initial value of β should be calculated according to the following formula:

[0028]

[0029] Among them, e max is the maximum value among the absolute values of the residuals corresponding to the observation quantities. In addition, the non-convexity parameter β1 corresponding to the GNSS observation quantity and the non-convexity parameter β2 corresponding to the vehicle-to-vehicle ranging should be calculated using the maximum value of the GNSS pseudorange double-difference residual and the maximum value of the vehicle-to-vehicle ranging residual, respectively;

[0030] S33. Replace the weights θ of each observation quantity in the solution equation of S2 with the weights of each observation quantity calculated in S32, and re-use this equation to estimate the vehicle state;

[0031] S34. Update the parameter β, where the parameter corresponding to the GNSS observation quantity is β1, and the parameter corresponding to the vehicle-to-vehicle ranging is β2. The update equation is:

[0032] β1 = 1.4 * β1

[0033] β2 = 1.4 * β2

[0034] S35. Repeat steps S32, S33, and S34.

[0035] The specific method of S4:

[0036] If, during the iteration process, the weights of all information related to GNSS pseudorange and GNSS carrier information and the weights of vehicle-to-vehicle ranging information all become 0 or 1, stop the loop iteration, use this weight to perform a vehicle state estimation once, and use this result as the final result of the vehicle state estimation.

[0037] Through the above steps, the influence of outliers in GNSS observation quantities and vehicle-to-vehicle ranging observation quantities on the overall vehicle positioning result can be effectively excluded, improving the accuracy and robustness of vehicle positioning.

[0038] According to the design of the present invention, the present invention realizes obtaining the position data of cooperative vehicles only by using vehicle-to-vehicle communication equipment, with low implementation cost. At the same time, this method has strong scalability and can be combined with multi-sensor methods to further improve the positioning accuracy and usability.

[0039] According to the design of the present invention, the present invention can detect faulty observation quantities without additional equipment, and the computational complexity is significantly reduced compared with existing methods, with a wider application range and higher usability. Description of the Drawings

[0040] Figure 1 is the method architecture diagram of an embodiment of the present invention.

[0041] Figure 2 is the flowchart of state estimation in the method of an embodiment of the present invention. Detailed Embodiments

[0042] In order to have a further understanding and knowledge of the features, purposes, and functions of the present invention, the present invention will be described in more detail below in conjunction with specific implementation examples and drawings.

[0043] First, the basic scenario and core principle of the present invention are outlined. Multiple vehicles are driving on a road with a flat surface. The vehicles can share their respective real-time position information through the V2X link. Specifically, the position here refers to the position of the phase center of the vehicle's GNSS main antenna. In addition to the vehicle position, the vehicles also share the height information of their respective GNSS antennas from the ground. When a vehicle receives the position information and antenna height information sent by a cooperative vehicle, the vehicle can use these data to fit a plane for vehicle driving. For the convenience of understanding the implementation process of the proposed method, a target vehicle is defined as the research object in the present invention, and the remaining vehicles are regarded as its collaborators. Since the method is distributed, each vehicle can use the method proposed in the present invention in actual applications.

[0044] As Figure 1 shown, it is the method architecture diagram of the present invention, which can be divided into two major parts: data acquisition and processing, and state estimation. Among them, data acquisition and processing include the acquisition of vehicle raw information and the conversion into the form of objective measurement residuals. State estimation is the core of the proposed method. The state estimation flowchart is as Figure 2 shown, and the factor graph optimization algorithm is specifically used to implement the solution of the position. The specific implementation is as follows:

[0045] The first step: Data acquisition and construction of objective measurement residual factors.

[0046] (1) Data acquisition

[0047] Obtain carrier phase, pseudorange, Doppler, elevation angle, and carrier-to-noise ratio data of different moments, different satellites, and different frequency bands from the GNSS raw observation file and navigation file. Perform preliminary processing, eliminate satellites with too low elevation angles, and select appropriate satellites as reference satellites for double difference.

[0048] (2) Conversion of each objective measurement into the form of relevant residuals

[0049] First, the vehicle state is:

[0050] χ = [X m,1 , X m,2 , …, Xm,t

[0051]

[0052] X m,t represents the vehicle state at different times. N represents the vehicle position and the double-difference ambiguity of each carrier respectively.

[0053] At this time, the GNSS pseudorange double-difference residual is expressed as:

[0054]

[0055]

[0056] is the pseudorange double-difference calculated from the observed quantities. are the satellite position, the reference satellite position and the reference base station position respectively.

[0057] The GNSS carrier double-difference residual is expressed as:

[0058]

[0059]

[0060] λ is the wavelength of the carrier. is the carrier double-difference calculated from the observed quantities. are the satellite position, the reference satellite position and the reference base station position respectively.

[0061] The Doppler velocity residual is expressed as:

[0062]

[0063] v dopp,m,t is the vehicle speed calculated from the observed quantities, Δt represents the time interval between two consecutive times (in seconds), and p m,t represents the position of vehicle m at time t.

[0064] The inter-vehicle ranging residual is expressed as:

[0065]

[0066] represents the vehicle ranging information obtained by the ultra-wideband sensor. represents the position information of cooperative vehicle m coop in the ECEF coordinate system at time t.

[0067] Step 2: State estimation

[0068] (1) Calculate the vehicle state​

[0069] The vehicle state is estimated using a factor graph optimization algorithm that fuses GNSS data and vehicle-to-vehicle ranging data. The estimation equation is as follows:

[0070]

[0071] where χ * is the optimal state of the vehicle, and represent the weights of GNSS pseudorange and carrier phase observations and the weight of vehicle-to-vehicle ranging observations at time t, respectively.

[0072] Before the first iteration, a positioning needs to be performed according to this formula. At this time, all weights need to be set to 1.

[0073] (2) Calculate the weights corresponding to each observation

[0074] The weights of GNSS observations and vehicle-to-vehicle ranging weights should be calculated separately. The magnitude of the weight value is related to the non-convex interval parameter c of the control robust function, the non-convexity parameter β of the control function, and the magnitude of the residual e of each observation. The specific form is as follows:

[0075]

[0076] where the weight is denoted as θ. The weights of GNSS observations and vehicle-to-vehicle ranging observations should be calculated according to their corresponding non-convex interval parameter c of the control robust function and non-convexity parameter β of the control function, respectively.

[0077] Before the first iteration, the initial value of β should be calculated according to the following formula:

[0078]

[0079] where e max is the maximum value of the absolute value of the residual corresponding to the observation. In addition, the non-convexity parameter β1 corresponding to GNSS observations and the non-convexity parameter β2 corresponding to vehicle-to-vehicle ranging should be calculated using the maximum value of GNSS pseudorange double-difference residuals and the maximum value of vehicle-to-vehicle ranging residuals, respectively.

[0080] (3) Update of the non-convexity control parameter of the robust function

[0081] After calculating the weights corresponding to each observation, the parameter β should be updated. Among them, the parameter corresponding to GNSS observations is β1, and the parameter corresponding to vehicle-to-vehicle ranging is β2. The update equations are as follows:

[0082] β1 = 1.4 * β1

[0083] β2 = 1.4 * β2

[0084] (4) Iterative update

[0085] After calculating the latest parameter β and the weights corresponding to each observable, the process in the second step should be repeated to recalculate the optimal state of the vehicle, recalculate the residuals of each observable, recalculate the weights, and update the parameter β.

[0086] If all weights become 0 or 1 during the iteration process, stop the loop iteration, perform a vehicle state estimation using these weights, and use the result as the final result of the vehicle state estimation.

[0087] The above description is only for the embodiments of the present application and is not intended to limit the present application. For those skilled in the art, various changes and modifications can be made to the present application. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application shall be included within the scope of the claims of the present application.

Claims

1. A method for suppressing outliers and performing collaborative positioning using a progressive non-convex robust function, characterized in that: It includes the following steps: S1. Obtain the GNSS raw observation data and vehicle - to - vehicle ranging information carried by the vehicle, and transform this information into the form of residuals of each observable quantity; S2. Use the factor graph algorithm to fuse the residual data of each observable quantity from different sensors and at different times; S3. Select the truncated least - squares function assisted by the progressive non - convex method as the robust function in the M - estimation, perform weighted processing on the vehicle GNSS carrier double - difference observable quantity, GNSS pseudorange double - difference observable quantity, and vehicle - to - vehicle ranging observable quantity, and iterate continuously to solve the optimal state of the vehicle and the optimal weights of each observable quantity in turn; Specifically, it includes the following sub - steps: S31. Before iteration, first set the weight of each observable quantity to 1, and solve the vehicle state according to the solution equation in S2; S32. Calculate the GNSS pseudorange double - difference residual and the vehicle - to - vehicle ranging value residual according to the vehicle position calculated in the previous step. The specific calculation formulas are as follows: Among them, the weight is represented as θ. The weights of the GNSS observable quantity and the vehicle - to - vehicle ranging observable quantity should be calculated according to the non - convex interval parameter c of its corresponding control robust function and the non - convexity parameter β of the control function respectively; If it is the first iteration, the initial value of β should be calculated according to the following formula: Among them, e max is the maximum value among the absolute values of the residuals corresponding to the observed quantities. In addition, the non-convexity parameter β1 corresponding to the GNSS observed quantity and the non-convexity parameter β2 corresponding to the vehicle-to-vehicle ranging should be calculated using the maximum value of the GNSS pseudorange double-difference residual and the maximum value of the vehicle-to-vehicle ranging residual, respectively; S33. Replace the weight θ of each observable quantity in the solution equation of S2 with the weight of each observable quantity calculated in S32, and re - use this equation to estimate the vehicle state; S34. Update the parameter β, where the parameter corresponding to the GNSS observable quantity is β1, and the parameter corresponding to the vehicle - to - vehicle ranging is β2. The update equation is: Among them, k is the number of iterations; S35. Repeat steps S32, S33, and S34; S4. When, during the iteration, the weights corresponding to each observable quantity finally become 0 or 1, stop the iteration, and output the positioning result at this time as the final vehicle position.

2. The method for suppressing outliers in collaborative positioning using a progressive non-convex robust function according to claim 1, wherein; The said S1 includes the following sub - steps: S11. Obtain the vehicle GNSS raw data and the reference base station GNSS raw data, extract and save the pseudorange, elevation angle, carrier, Doppler, and carrier - to - noise ratio information; S12. Obtain and save the vehicle - to - vehicle ranging information obtained by the ultra - wideband UWB sensor at the current moment and the position information of cooperative vehicles obtained through the vehicle - to - everything network. At the same time, convert the vehicle position information into the coordinates in the ECEF coordinate system; S13. Calculate the accurate positions of each GNSS satellite in the ECEF coordinate system at this time according to the ephemeris file and save them; S14. Select the satellite position information at the current moment and previous moments, the GNSS raw data information saved in S11, and the vehicle - to - vehicle ranging information saved in S13 according to the window length set by the factor graph algorithm; Among the data at the same moment, eliminate all satellites with too small elevation angles in the GNSS raw information, and select the satellite with the largest carrier - to - noise ratio that is common to the base station and the vehicle as the double - difference reference satellite; S15. According to the information at different times screened in S14, first use the GNSS pseudorange to calculate the positions of the vehicle at different times, and then use the Doppler data to calculate the vehicle speed at the current moment; S16. Set the vehicle positions at different times calculated in S15 as the initial values of the vehicle state, and calculate the pseudorange double-difference residuals, carrier double-difference residuals, Doppler velocity residuals, and ultra-wideband vehicle ranging residuals respectively by combining the information selected in S14.

3. The method for suppressing outliers and collaborative positioning using a progressive non-convex robust function according to claim 1, wherein; The specific process of S2 is as follows: In the factor graph algorithm, the GNSS pseudorange double-difference, GNSS carrier double-difference, GNSS velocity, and ultra-wideband vehicle ranging information at the current time and multiple previous times are fused. The specific solution equation is as follows: Among them, χ * is the optimal state of the vehicle; and e m,dv,t represent the GNSS pseudorange double-difference residual, GNSS carrier double-difference residual, ultra-wideband vehicle-to-vehicle ranging residual, and GNSS velocity residual factor at time t, respectively; and represent the weights related to the GNSS pseudorange and carrier observables and the weight of the vehicle-to-vehicle ranging observation at time t, respectively.

4. The method for suppressing outliers in collaborative positioning using a progressive non-convex robust function according to claim 1, characterized in that; The specific method of S4: If, during the iterative process, the weights of all information related to GNSS pseudorange and GNSS carrier and the weights of vehicle ranging information all become 0 or 1, stop the loop iteration, perform a vehicle state estimation using this weight, and use this result as the final result of the vehicle state estimation.

Images