Method for carrying out suppression co-localization on outliers by using progressive non-convex robust function

Abstract

The invention belongs to the field of satellite navigation, and discloses a method for carrying out suppression co-localization on outliers by using a progressive non-convex robust function, which comprises the following steps of: firstly, acquiring original data from a sensor carried by a vehicle, and representing the original data in a form about each observed quantity residual error after processing; secondly, in a factor graph algorithm architecture, performing robust estimation on GNSS pseudo-range double-difference, GNSS carrier double-difference and inter-vehicle distance measurement values by using a truncated least square (TLS) function assisted by a progressive non-convex method, and alternately solving a vehicle state and each observed quantity weight; and finally, after multiple iterations, when the weights corresponding to the observed quantities are all changed into 0 or 1, stopping the iterations, and outputting an estimation result of the vehicle state at the moment as a final positioning solution of the vehicle. According to the method, the observed quantity with the fault can be detected without additional equipment, the calculation amount is remarkably reduced compared with that of an existing method, the application range is wider, and the usability is higher.

Description

Technical Field

[0001] The invention belongs to the field of satellite navigation, and in particular relates to a method for suppressing collaborative positioning of outliers by using asymptotically non-convex robust functions. 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 performing collaborative positioning, vehicles can not only use local measurement data, but also use measurement data shared by collaborative vehicles. Compared with the positioning method of single vehicle multi-sensor, the collaborative positioning method has the following advantages: the measurement values ​​from the collaborative node can improve the geometric precision factor and enhance the positioning accuracy; the additional measurement values ​​can increase the observation dimension and the number of positioning equations, and enhance the availability of positioning; most collaborative positioning methods do not require expensive equipment such as lidar, and mainly use vehicle-mounted communications and low-cost ranging units, with very low implementation costs; they do not involve complex image processing and data matching, occupy less vehicle resources, and can ensure the real-time positioning. Existing collaborative positioning methods are all implemented on the basis of workshop measurements, and there is currently no collaborative positioning method that can be implemented without workshop measurements. However, due to constraints such as security, privacy, permissions, and equipment costs, not all collaborative vehicles have the ability to provide original measurement information, and the vehicle to be positioned may not be able to obtain the measurement data of the collaborative vehicle. In addition, if there is a large delay in the transmission of collaborative measurement data, the measurement data cannot be used for positioning solution at the current moment.

[0003] M-estimation is an estimation method in robust estimation, which is used to estimate parameters more reliably in the presence of outliers or noisy data. Traditional least squares estimation is more sensitive to outliers, while M-estimation suppresses the influence of outliers by introducing different loss functions, thereby improving the robustness of estimation. In collaborative positioning, it is necessary to obtain the original GNSS observation data and the ranging data between the nodes, but usually, due to various interferences, the observations may have large deviations. 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 observations, and by giving these observations less weight, reduce the impact of outliers on the overall positioning results.

[0004] Different from the collaborative localization method in the measurement domain, the collaborative localization method in the positioning domain only uses the position data of the cooperative vehicle to realize the positioning solution. Therefore, a collaborative localization method in the positioning domain is proposed, which uses the asymptotically non-convex truncated least squares function to weight each observation, which has important practical significance for improving the availability and robustness of vehicle positioning in urban environments. Summary of the invention

[0005] The purpose of the present invention is to use a progressively non-convex truncated least squares function to weight the sensor observations carried by the vehicle to improve the accuracy and robustness of vehicle positioning in urban environments. In dense urban environments, the obstruction of high-rise buildings, bridges and even trees will lead to the degradation of satellite observations or vehicle ranging. In addition, the probability of non-line-of-sight propagation or multipath effects of the signal is significantly increased. These faults will degrade the overall positioning accuracy of the vehicle, making it difficult to meet the positioning accuracy requirements of high-level vehicle applications.

[0006] The present invention proposes a method for suppressing outliers by using asymptotically non-convex robust functions for collaborative positioning, which solves the problem of positioning degradation due to occlusion by objects such as high-rise buildings in urban environments. The implementation steps are as follows:

[0007] S1. Obtain the original GNSS observation data and vehicle distance measurement information carried by the vehicle, and convert the information into the form of residual errors of each observation quantity;

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

[0009] S3. Select the truncated least squares function assisted by the asymptotically non-convex method as the robust function in the M-estimation, perform weighted processing on the vehicle GNSS carrier double-difference observations, GNSS pseudo-range double-difference observations and vehicle-to-vehicle ranging observations, iterate continuously, and solve the optimal state of the vehicle and the optimal weight of each observation in turn;

[0010] S4. When the weights corresponding to the observations finally become 0 or 1 during the iteration, the iteration is stopped and the positioning result at this time is output as the final vehicle position.

[0011] The “converting the information into the form of residuals of the observed quantities” mentioned in S1 includes GNSS pseudorange double difference residuals, GNSS carrier double difference residuals, GNSS Doppler velocity residuals and ultra-wideband (UWB) inter-vehicle ranging residuals, and the method is as follows:

[0012] S11, obtaining vehicle GNSS raw data and reference base station GNSS raw data, extracting and saving pseudorange, elevation angle, carrier, Doppler and carrier-to-noise ratio information;

[0013] S12, obtaining and saving the inter-vehicle distance measurement information obtained by the ultra-wideband (UWB) sensor at the current moment and the position information of the cooperative vehicle obtained through the Internet of Vehicles, and converting the vehicle position information into the coordinates of the ECEF coordinate system;

[0014] S13, calculating the accurate position of each GNSS satellite in the ECEF coordinate system at this time according to the ephemeris file and saving it;

[0015] S14. Select the satellite position information at the current moment and the previous moment, the GNSS raw data information saved in S11, and the vehicle distance measurement 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 with the largest carrier-to-noise ratio shared by the base station and the vehicle as the double-difference reference satellite;

[0016] S15, based on the information at different times selected by S14, firstly use the GNSS pseudorange to calculate the position of the vehicle at different times, and then use the Doppler data to calculate the speed of the vehicle at different times;

[0017] S16, setting the vehicle positions at different times solved by S15 as the initial values ​​of the vehicle state estimation in subsequent iterations, and combining the information screened out by S14 to calculate the pseudorange double difference residual, carrier double difference residual, Doppler velocity residual and ultra-wideband vehicle ranging residual.

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

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

[0020]

[0021] Among them, χ * The vehicle is in optimal condition; and e m,dv,t They represent the GNSS pseudorange double difference residual, GNSS carrier double difference residual, ultra-wideband workshop ranging residual, and GNSS velocity residual factor at time t respectively; and They represent the weights of the GNSS pseudorange and carrier observations and the weights of the vehicle ranging observations at time t respectively.

[0022] The S3 specifically includes the following sub-steps:

[0023] S31, before iteration, first reset the weight of each observation to 1, and solve the vehicle state according to the solution equation of S2;

[0024] S32. Calculate the GNSS pseudo-range double difference residual and the vehicle distance measurement value residual according to the vehicle position calculated in the previous step. The specific calculation formula is as follows:

[0025]

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

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

[0028]

[0029] Among them, e max is the maximum value of the absolute value of the residual corresponding to the observation. In addition, the non-convex parameter β1 corresponding to the GNSS observation and the non-convex parameter β2 corresponding to the workshop ranging should be calculated using the maximum value of the GNSS pseudorange double difference residual and the maximum value of the workshop ranging residual respectively.

[0030] S33, replacing each observation weight θ in the solution equation of S2 with each observation weight calculated by S32, and reusing the equation to estimate the vehicle state;

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

[0032] β1=1.4*β1

[0033] β2=1.4*β2

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

[0035] The S4 specific method:

[0036] If during the iteration process, all weights of the GNSS pseudorange and GNSS carrier information and the weight of the vehicle ranging information become 0 or 1, the loop iteration is stopped, and the vehicle state estimation is performed once using the weight, and the result is used as the final result of the vehicle state estimation.

[0037] Through the above steps, the influence of outliers in GNSS observations and vehicle-to-vehicle ranging observations on the overall vehicle positioning results can be effectively eliminated, thereby improving the accuracy and robustness of vehicle positioning.

[0038] According to the design of the present invention, the present invention realizes the use of only workshop communication equipment to obtain the location data of cooperative vehicles, which has low implementation cost. At the same time, the method has strong scalability and can be combined with a multi-sensor method to further improve positioning accuracy and availability.

[0039] According to the design of the present invention, the present invention can detect the observed quantity with faults without the assistance of additional equipment, and the amount of calculation is significantly reduced compared with the existing method, with a wider scope of application and higher availability. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0041] Figure 2 It is a flow chart of state estimation in the method of an embodiment of the present invention. DETAILED DESCRIPTION

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

[0043] First, the basic scenarios and core principles of the present invention are outlined. When multiple vehicles are traveling on a smooth road, the vehicles can share their respective real-time location information through the V2X link. Specifically, the location here refers to the location of the phase center of the vehicle's GNSS main antenna. In addition to the vehicle's location, the vehicles also share the height information of their respective GNSS antennas from the ground. When a vehicle receives the location information and antenna height information sent by the collaborative vehicle, the vehicle can use these data to fit a plane on which the vehicle is traveling. In order to facilitate understanding of 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] like Figure 1 As shown in the figure, it is the architecture diagram of the method of this invention, which can be divided into two parts: data acquisition and processing and state estimation. Among them, data acquisition and processing include the acquisition of the original information of the vehicle and its conversion into the form of residuals of each observation. State estimation is the core of the proposed method. The state estimation flow chart is shown in Figure 2 As shown, the factor graph optimization algorithm is specifically used to realize the position solution. The specific implementation is as follows:

[0045] The first step: data collection and construction of residual factors for each observation.

[0046] (1) Data Collection

[0047] The carrier phase, pseudorange, Doppler, elevation angle, and carrier-to-noise ratio data of different satellites and frequency bands at different times are obtained from the original GNSS observation files and navigation files. Preliminary processing is performed to remove satellites with too low elevation angles and select appropriate satellites as reference satellites for double difference.

[0048] (2) Each observation is transformed into the form of relevant residuals

[0049] First, the vehicle status is:

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

[0051]

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

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

[0054]

[0055]

[0056] is the double difference of pseudorange calculated based on the observed values, Ask for satellite position, reference satellite position and 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 based on the observed value, Ask for satellite position, reference satellite position and reference base station position respectively.

[0061] The Doppler velocity residual is expressed as:

[0062]

[0063] v dopp,m,t is the vehicle speed calculated based on the observed value, Δt represents the interval between the two moments (in seconds), and p m,t Represents the position of vehicle m at time t.

[0064] The workshop ranging residual is expressed as:

[0065]

[0066] Represents the vehicle ranging information obtained by the ultra-wideband sensor, Indicates that at time t, the number of cooperative vehicles m coop Position information in the ECEF coordinate system.

[0067] Step 2: State Estimation

[0068] (1) Calculate vehicle status

[0069] The vehicle state is estimated by using the factor graph optimization algorithm that integrates GNSS data and vehicle-to-vehicle distance measurement data. The estimation equation is:

[0070]

[0071] Among them, χ * To achieve the optimal vehicle condition, and They represent the weights of the GNSS pseudorange and carrier observations and the weights of the vehicle ranging observations at time t respectively.

[0072] Before the first iteration, 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 GNSS observation weights and vehicle ranging weights should be calculated separately. The weight values ​​are related to the non-convex interval parameter c of the control robust function, the non-convex parameter β of the control function and the size of the residual e of each observation. The specific form is:

[0075]

[0076] Among them, the weight is expressed as θ, and the GNSS observation weight and the vehicle ranging observation weight should be calculated according to their corresponding control robust function non-convex interval parameter c and control function non-convexity parameter β respectively;

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

[0078]

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

[0080] (3) Updating of non-convex control parameters of robust functions

[0081] After calculating the weights corresponding to each observation, the parameter β should be updated. The parameter corresponding to the GNSS observation is β1, and the parameter corresponding to the vehicle ranging is β2. The update equation is:

[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 observation, the process in the second step should be repeated to recalculate the optimal state of the vehicle, recalculate the residuals of each observation, recalculate the weights, and update the parameter β.

[0086] If all weights become 0 or 1 during the iteration process, the loop iteration is stopped, the vehicle state is estimated once using the weight, and the result is used as the final result of the vehicle state estimation.

[0087] The above is only an embodiment of the present application and is not intended to limit the present application. For those skilled in the art, the present application may have various changes and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application should be included in the scope of the claims of the present application.

Claims

1. A method for suppressing outliers and co-localizing using asymptotically non-convex robust functions, characterized in that: The following steps are involved: S1. Obtain the original GNSS observation data and vehicle distance measurement information carried by the vehicle, and convert the information into the form of residual errors of each observation quantity; S2, using the factor graph algorithm to fuse the residual data of each observation quantity from different sensors and at different times; S3. Select the truncated least squares function assisted by the asymptotically non-convex method as the robust function in the M-estimation, perform weighted processing on the vehicle GNSS carrier double-difference observations, GNSS pseudo-range double-difference observations and vehicle-to-vehicle ranging observations, iterate continuously, and solve the optimal state of the vehicle and the optimal weight of each observation in turn; S4. When the weights corresponding to the observations finally become 0 or 1 during the iteration, the iteration is stopped and the positioning result at this time is output as the final vehicle position.

2. The method for suppressing co-localization of outliers using asymptotically non-convex robust functions according to claim 1, characterized in that: The S1 comprises the following sub-steps: S11, obtaining vehicle GNSS raw data and reference base station GNSS raw data, extracting and saving pseudorange, elevation angle, carrier, Doppler and carrier-to-noise ratio information; S12, obtaining and saving the inter-vehicle distance measurement information obtained by the ultra-wideband UWB sensor at the current moment and the position information of the cooperative vehicle obtained through the Internet of Vehicles, and converting the vehicle position information into the coordinates of the ECEF coordinate system; S13, calculating the accurate position of each GNSS satellite in the ECEF coordinate system at this time according to the ephemeris file and saving it; S14, selecting the satellite position information at the current time and the previous time, the GNSS raw data information saved in S11, and the vehicle distance measurement information saved in S13 according to the window length set by the factor graph algorithm; In the data at the same time, all satellites with too small elevation angles in the original GNSS information are removed, and the satellite with the largest carrier-to-noise ratio shared by the base station and the vehicle is selected as the double-difference reference satellite; S15, based on the information at different times selected by S14, firstly use the GNSS pseudorange to calculate the position of the vehicle at different times, and then use the Doppler data to calculate the vehicle speed at the current time; S16, setting the vehicle positions at different times solved by S15 as the initial values ​​of the vehicle state, and combining the information screened out by S14 to calculate the pseudorange double difference residual, carrier double difference residual, Doppler velocity residual and ultra-wideband vehicle ranging residual.

3. The method for suppressing co-localization of outliers using asymptotically non-convex robust functions according to claim 1, characterized in that: The specific process of S2 is as follows: The factor graph algorithm integrates the GNSS pseudorange double difference, GNSS carrier double difference, GNSS speed, and ultra-wideband vehicle ranging information at the current moment and multiple previous moments. The specific solution equation is as follows: Among them, χ is the optimal state of the vehicle; and They represent the GNSS pseudorange double difference residual, GNSS carrier double difference residual, ultra-wideband workshop ranging residual, and GNSS velocity residual factor at time t respectively; and They represent the weights of the GNSS pseudorange and carrier observations and the weights of the vehicle ranging observations at time t respectively.

4. The method for suppressing co-localization of outliers using asymptotically non-convex robust functions according to claim 3, characterized in that: The S3 specifically includes the following sub-steps: S31, before iteration, first reset the weight of each observation to 1, and solve the vehicle state according to the solution equation of S2; S32. Calculate the GNSS pseudo-range double difference residual and the vehicle distance measurement value residual according to the vehicle position calculated in the previous step. The specific calculation formula is as follows: Among them, the weight is expressed as θ, and the GNSS observation weight and the vehicle ranging observation weight should be calculated according to their corresponding control robust function non-convex interval parameter c and control function non-convexity parameter β respectively; If it is the first iteration, the initial β value should be calculated according to the following formula: Among them, e max is the maximum value of the absolute value of the residual corresponding to the observation. In addition, the non-convex parameter β1 corresponding to the GNSS observation and the non-convex parameter β2 corresponding to the workshop ranging should be calculated using the maximum value of the GNSS pseudorange double difference residual and the maximum value of the workshop ranging residual respectively. S33, replacing each observation weight θ in the solution equation of S2 with each observation weight calculated by S32, and reusing the equation to estimate the vehicle state; S34, update the parameter β, where the parameter corresponding to the GNSS observation is β1, and the parameter corresponding to the vehicle ranging is β2, and the update equation is: β1＝1.4*β1 β2＝1.4*β2 S35. Repeat steps S32, S33, and S34.

5. The method for suppressing co-localization of outliers using asymptotically non-convex robust functions according to claim 4, characterized in that: The S4 specific method: If during the iteration process, all weights of the GNSS pseudorange and GNSS carrier information and the weight of the vehicle ranging information become 0 or 1, the loop iteration is stopped, and the vehicle state estimation is performed once using the weight, and the result is used as the final result of the vehicle state estimation.

Images