High-precision positioning algorithm for autonomous driving vehicles on roundabouts based on time synchronization algorithm

Abstract

The present invention discloses a high-precision positioning algorithm for autonomous driving vehicles under roundabouts based on a time synchronization algorithm, including: the autonomous driving vehicle broadcasts CAM information to an intelligent base station in the kth broadcast cycle; after receiving the CAM message, the intelligent base station estimates the current geographical location of the autonomous driving vehicle; the autonomous driving vehicle determines: for any ε>0, if the difference between the estimated value of the current geographical location of the autonomous driving vehicle by the intelligent base station and the estimated value of the autonomous driving vehicle's own geographical location is less than ε, the positioning is completed; otherwise, the local clock of the autonomous driving vehicle is updated through the time synchronization algorithm, and its own geographical location is estimated. This algorithm designs the C‑ATS algorithm, and is used for the coordinated synchronization of the clock and positioning of the intelligent base station and the autonomous driving vehicle, to overcome the influence of asymmetric communication delay on positioning accuracy, improve the time synchronization accuracy, and improve the accuracy of vehicle-road collaborative positioning.

Description

Technical Field

[0001] The present invention belongs to the field of high-precision positioning technology, and relates to a method for high-precision positioning of an autonomous driving vehicle in a roundabout scenario using a vehicle-road cooperative technology, and specifically to a high-precision positioning algorithm for an autonomous driving vehicle in a roundabout scenario based on a time synchronization algorithm. Background Art

[0002] With the development of current assisted autonomous driving and future unmanned driving technology, autonomous driving vehicles have entered the commercial pilot landing stage, and the technical requirements of autonomous driving vehicles are constantly improving. Among them, high-precision positioning can help autonomous driving vehicles obtain accurate location information, enable autonomous driving vehicles to achieve precise control, and complete complex behaviors, which has become a research hotspot in the field of autonomous driving.

[0003] As an important traffic node, city roundabouts are often set up at the intersection of multiple traffic intersections, allowing vehicles to travel in the same direction, which can effectively reduce traffic accidents and improve traffic rates. However, with the increase in traffic volume, the number of lanes on the road at the roundabout can often reach three or four lanes. At the same time, roundabouts are often located in the center of a bustling city, and tall buildings will form a canyon effect, blocking GPS signals. Therefore, roundabouts are a typical scenario for achieving high-precision positioning of autonomous vehicles.

[0004] However, the existing vehicle positioning technology has the following two problems: First, it cannot meet the lane-level positioning requirements of autonomous vehicles, causing traffic accidents due to inaccurate positioning of autonomous vehicles. Second, in an environment where GPS signals are missing, the positioning deviation of autonomous vehicles may be too large or even impossible to locate. Therefore, how to achieve high-precision lane-level positioning of autonomous vehicles in roundabout environments has become a technical problem in the field of autonomous driving.

[0005] The scene of the autonomous driving vehicle missing positioning in the roundabout is shown in the figure Figure 1 As shown in the figure, multiple autonomous vehicles drove into the roundabout, but the tall trees in the center of the roundabout and on the roadside, as well as the buildings in the city blocked the GPS signal, resulting in increased positioning deviation when the autonomous vehicles used a single GPS positioning, making it impossible for the autonomous vehicles to make correct driving decisions, thus causing serious traffic accidents. Figure 1 Two typical traffic accidents caused by positioning deviation of autonomous driving vehicles are introduced in this paper. Figure 1 In the upper right corner, the autonomous driving vehicle drove into the opposite lane due to positioning deviation when leaving the roundabout, causing it to drive in the opposite direction and cause a serious collision with a normal vehicle in the opposite lane. Figure 1When the autonomous driving vehicle in the lower right corner was leaving the roundabout, it misidentified the inner circle as the outer lane due to its own geographical positioning deviation, and issued a command to leave the roundabout, resulting in a serious collision with the car in the outer circle.

[0006] Existing autonomous driving vehicle positioning technologies can be divided into Global Navigation Satellite System (GNSS), visual positioning, dead reckoning positioning and vehicle-road collaborative positioning.

[0007] Among them, vehicle-road collaborative positioning technology is a wireless cooperative positioning method based on vehicle-mounted ad hoc networks (VANET), that is, each vehicle and intelligent roadbed exchange their observed road information through wireless communication, and correct the current vehicle positioning result based on the information obtained from the interaction to improve the accuracy of vehicle positioning. Vehicle-road collaborative positioning does not rely on satellite positioning equipment and is less affected by surrounding environmental factors. It can effectively solve the problems of low real-time positioning, poor effective accuracy, and weak anti-interference ability of vehicles in the absence of satellite positioning signals. It has become the development trend of future vehicle autonomous positioning.

[0008] In vehicle-road collaborative positioning, since the sensors between vehicles or between vehicles and base stations are implemented by different hardware, each sensor has its own specific clock, and the clock in the sensor is highly sensitive, resulting in asynchrony between the precise clocks of the vehicle and the base station. Slight differences in the clocks on the nodes may cause serious positioning errors. Therefore, how to synchronize the time between the vehicle and the roadbed has become the core of the vehicle-road collaborative positioning algorithm. At present, the main time synchronization algorithms used in vehicle speed collaborative positioning are the Average Time Synchronization algorithm (ATS), the Weighted Maximum Time Synchronization algorithm (WMTS) and the Least-Square estimation Time Synchronization algorithm (LSTS).

[0009] The main idea of ​​the ATS algorithm is that each node achieves clock synchronization of the entire network by averaging the logical clocks of itself and its neighboring nodes. Compared with the Distributed Consensus Time Synchronization (DCTS) algorithm, the ATS algorithm achieves the convergence of time synchronization error under delay-free conditions, but the time synchronization error will diverge when there is a random communication delay. Gang X et al. proposed the DCTS algorithm and calculated the asymptotic expectation and mean square of the synchronization error. L. Schenat et al. proposed the ATS algorithm and gave a rigorous proof that the ATS algorithm converges in an ideal environment without communication delay. Tian et al. proved that the ATS algorithm is almost bound to diverge in an environment with random communication delays in an undirected topology wireless sensor network (WSN). Wang, H et al. proposed a relative skewness estimator to reduce communication delays, so that the ATS algorithm can achieve time synchronization in the presence of communication delays.

[0010] The WMTS algorithm is based on the Maximum Time Synchronization (MTS) protocol. The main idea is to synchronize all nodes in the entire network to the node with the fastest clock, that is, to regard the node with the fastest clock as the reference node. Unlike the MTS algorithm, when updating the node logical clock, the process of assigning weights according to the size of the logical clocks of adjacent nodes is added, rather than a simple average random approximation. The WMTS algorithm can effectively handle random communication delays and improve the convergence speed. R.Olfati-Saber et al. proposed the definition and strategy of consistency consensus. Jianping He et al. first proposed the MTS algorithm and proved that the algorithm converges in a finite time. They also proposed the WMTS algorithm for the problem of WSN with random communication delays, and gave a convergence proof and an upper bound of the finite convergence time, indicating that the convergence time is much shorter than the ATS algorithm. Tian et al. proved that the WMTS algorithm is mean square divergent in an environment with random communication delays, and its synchronization accuracy depends on the mean and mean square error of the random delays.

[0011] The LSTS algorithm is similar to the ATS algorithm. The main idea is to average the logical clocks of the nodes and their neighbors to achieve clock synchronization for the entire network. However, the LSTS algorithm proposes a new method for estimating relative clock speeds—least squares iterative estimation—and introduces an attenuation factor. Huang et al. proposed an LSTS algorithm that gradually reduces the step size, and introduced the concepts of mean square and strong consensus, proving that the LSTS algorithm can achieve asymptotic convergence of time synchronization in the presence of random communication time delays. Tian et al. proved in 2015 that the LSTS algorithm overcomes the mean square divergence condition of the consensus-based time synchronization algorithm (CBTS), and that the time synchronization error and offset compensation of the LSTS algorithm are bounded. Tian et al. proposed a delay compensation-based time synchronization algorithm (DCBTS) in 2021, introduced a delay compensation mechanism, adjusted the delay compensation through the first-order difference of the offset estimate, and proved its effectiveness.

[0012] ATS, WMTS and LSTS algorithms can achieve different degrees of clock synchronization under bounded communication delay. However, the transmission of synchronization information will also produce different amounts of time delay due to different transmission distances. For moving vehicles, the time delay caused by asymmetric transmission will continue to accumulate, which is also one of the important reasons for the divergence of vehicle positioning synchronization. However, the above time synchronization algorithms cannot achieve clock synchronization when there is asymmetric communication delay. Summary of the invention

[0013] The technical problem to be solved by the present invention is to provide a high-precision positioning algorithm for autonomous driving vehicles on roundabouts based on a time synchronization algorithm in response to the above-mentioned deficiencies in the prior art. This algorithm combines kinematic formulas and time synchronization to perform relative positioning of autonomous driving vehicles, designs a C-ATS algorithm, and innovatively uses it for the coordinated synchronization of clocks and positioning between intelligent base stations and autonomous driving vehicles, overcomes the impact of asymmetric communication delays on positioning accuracy, improves time synchronization accuracy, and thereby improves the accuracy of vehicle-road collaborative positioning, achieving lane-level high-precision positioning of autonomous driving vehicles in roundabout environments.

[0014] In order to achieve the above technical objectives, the technical solution adopted by the present invention is:

[0015] The high-precision positioning algorithm for autonomous driving vehicles under roundabouts based on the time synchronization algorithm includes:

[0016] Step 1: Install an intelligent base station at the center of the roundabout. The intelligent base station can obtain accurate time information. The intelligent base station and the autonomous driving vehicle c iThey can interact with each other through wireless communication in the form of periodic broadcasting;

[0017] Step 2: Autonomous driving vehicle c i In the first broadcast cycle, a CAM message is broadcast to the intelligent base station. The CAM message includes the current autonomous driving vehicle c i The local clock Instantaneous speed and autonomous vehicles i On local clock Geographic location valuation

[0018] Step 3: The intelligent base station receives the autonomous driving vehicle c i After the CAM message is broadcast, the autonomous driving vehicle c is obtained from the CAM message i Current location estimates

[0019] Step 4: Autonomous driving vehicle c i By pre-setting the drift correction factor and offset correction factor Value versus time Make corrections and get Will Assign to

[0020] Step 5: Autonomous driving vehicle c i In the k≥2th broadcast cycle, a CAM message is broadcasted to the intelligent base station. The CAM information includes the current autonomous driving vehicle c i The local clock Instantaneous speed and autonomous vehicles i Valuation of your geographic location in:

[0021]

[0022] in, represents the autonomous vehicle c i The random error in estimating its own geographic location during the rth broadcast cycle;

[0023] Step 6: The intelligent base station receives the autonomous driving vehicle c i After broadcasting the CAM message, Time, for autonomous driving vehicles c i Estimate the current geographic location and get the estimated value in:

[0024]

[0025] in, It means that the intelligent base station sends a signal to the autonomous driving vehicle c in the rth broadcast cycle. i Random errors in geolocation estimates, Indicates the local clock of the intelligent base station;

[0026] Smart base stations will be valued It is broadcasted in the CAM message;

[0027] Step 7: Autonomous Driving Vehicle c i The time of receiving the CAM message broadcast by the intelligent base station is Autonomous driving vehicles i Judgment: For any given value ε>0, if The positioning is completed. For autonomous vehicles i On local clock The exact geographical location of Then execute step 8;

[0028] Step 8: Autonomous Driving Vehicle c i The filter proportional factor is calculated according to the C-ATS algorithm, and the drift correction coefficient is calculated through the filter proportional factor and offset correction factor

[0029] Step 9: Using the Drift Correction Factor and offset correction factor For local clock Make corrections and get Will Assign to

[0030] Step 10: k=k+1; return to execute step 6.

[0031] As a further improved technical solution of the present invention, the instantaneous speed or The method to obtain is:

[0032] Autonomous driving vehicles i The wheel speed sensor on the vehicle is used to obtain the instantaneous speed measurement value, and the adaptive Kalman filter algorithm is used to filter the instantaneous speed measurement value to obtain the updated instantaneous speed. or

[0033] As a further improved technical solution of the present invention, in step 4:

[0034] As a further improved technical solution of the present invention, the filtering scale factor in step 8 is for:

[0035]

[0036] Among them, ρ η is the weight coefficient of the filter scale factor.

[0037] As a further improved technical solution of the present invention, the offset correction coefficient in step 8 is for:

[0038]

[0039] Among them, ρ o is the bias weight coefficient.

[0040] As a further improved technical solution of the present invention, the drift correction coefficient in step 8 is for:

[0041]

[0042] Among them, ρ α is the drift weight coefficient.

[0043] As a further improved technical solution of the present invention, by using the drift correction coefficient and offset correction factor For local clock Make corrections and get Specifically:

[0044] The beneficial effects of the present invention are:

[0045] The present invention does not rely on GPS positioning, but combines kinematic formulas and time synchronization models to perform relative positioning of autonomous driving vehicles. It establishes an autonomous driving vehicle-intelligent roadbed collaborative interactive positioning model in a roundabout scenario, and then designs a C-ATS algorithm based on ATS improved weights, and innovatively uses it for the clock and positioning collaborative synchronization of intelligent base stations and autonomous driving vehicles to overcome the impact of asymmetric communication delays on positioning accuracy. In addition, the adaptive Kalman algorithm and the time synchronization algorithm C-ATS are designed to improve the accuracy of time synchronization while further reducing the error of vehicle speed estimation, improving the accuracy of vehicle-road collaborative positioning, and achieving lane-level high-precision positioning of autonomous driving vehicles in roundabout scenarios. BRIEF DESCRIPTION OF THE DRAWINGS

[0046] Figure 1 This is a scene diagram of the missing positioning of the autonomous driving vehicle in the roundabout.

[0047] Figure 2 It is the time deviation graph of ATS and WMTS algorithms.

[0048] Figure 3 It is the time deviation graph of LSTS and C-ATS algorithms.

[0049] Figure 4 It is a diagram of standard vehicle speed and measured vehicle speed under six standard operating conditions of automobile cycle driving.

[0050] Figure 5 It is the vehicle speed error map processed by the algorithm.

[0051] Figure 6 It is a single positioning deviation diagram.

[0052] Figure 7 It is the positioning deviation diagram every 10 seconds.

[0053] Figure 8 It is the cumulative positioning deviation graph.

[0054] Fig. 9 It is the cumulative absolute value positioning deviation graph.

[0055] Fig.10 It is the positioning deviation map before filtering.

[0056] Fig.11 It is the positioning deviation map after the vehicle speed is processed by the standard Kalman algorithm.

[0057] Fig.12 It is the positioning deviation map after the vehicle speed is processed by the adaptive Kalman algorithm.

[0058] Fig.13 It is the positioning deviation diagram of LSTS and C-ATS at precise vehicle speed.

[0059] Fig.14 This is the positioning deviation diagram of LSTS and C-ATS under 1% vehicle speed random error.

[0060] Fig.15 This is the LSTS and C-ATS positioning deviation diagram after 1% vehicle speed random error filtering.

[0061] Fig.16 This is the LSTS and C-ATS positioning deviation diagram after 5% vehicle speed random error filtering. DETAILED DESCRIPTION

[0062] The specific embodiments of the present invention are further described below according to the accompanying drawings:

[0063] This embodiment provides a high-precision positioning algorithm for autonomous driving vehicles in roundabouts based on a time synchronization algorithm. The algorithm combines kinematic formulas and time synchronization models to perform relative positioning of autonomous driving vehicles, establishes a collaborative interactive positioning model of autonomous driving vehicles and intelligent roadbeds in roundabout scenarios, and then designs a C-ATS algorithm based on ATS improved weights, and innovatively uses it for the coordinated synchronization of clocks and positioning between intelligent base stations and autonomous driving vehicles to overcome the impact of asymmetric communication delays on positioning accuracy. It focuses on mathematically deriving the convergence characteristics of the C-ATS algorithm for two-dimensional positioning under asymmetric transmission paths, proving that the C-ATS algorithm can achieve higher positioning accuracy. In addition, an adaptive Kalman algorithm and a time synchronization algorithm C-ATS are designed to combine, while improving the accuracy of time synchronization, further reduce the error of vehicle speed estimation, and improve the accuracy of vehicle-road collaborative positioning, achieving lane-level high-precision positioning of autonomous driving vehicles in roundabout scenarios.

[0064] 1. Model establishment:

[0065] An intelligent base station is installed at the center of the roundabout (0,0). The intelligent base station is connected to the wired network and can obtain accurate clock information. Both the intelligent base station and the autonomous driving vehicle are equipped with wireless transceivers. The time is divided into several equally spaced time slots through self-organized time division multiplexing (STDMA), and cooperative awareness messages (CAM) are broadcast periodically. Each CAM message contains the instantaneous speed of the current sending node. Local clock and geographic location valuation The information is used to interactively share their respective operating status and road traffic safety information. According to the national standard C-ITS for on-board self-organizing network communication and the requirements of IEEE802.11p, it is stipulated that the time interval between adjacent intelligent units (vehicles, intelligent roadbeds, etc.) during the broadcast of CAM messages shall not be greater than 100 milliseconds, and the period of CAM message broadcast is set to T, that is, the broadcast period is T≤100ms.

[0066] The network topology between the autonomous driving vehicle and the intelligent base station is described by an undirected graph G = (V, E), where V = {c i |i≤n-1,n∈N +} represents all autonomous driving vehicles within the coverage of the intelligent base station broadcast, and the edge set E = {(i,j)|i≤n,j≤n,i,j∈N +} represents all wireless communication links that can realize information interaction within the broadcast coverage of the intelligent base station.

[0067] This paper adopts a typical linear clock model, defining t as real time and the autonomous driving vehicle c i The relationship between the clock reading τ and the real time t can be expressed as:

[0068]

[0069] where α i is the local clock drift, β i is the local clock deviation. The smart base station can obtain accurate time, so its local clock can be considered as absolute time. The relationship between the clock reading of the smart base station and the real time is

[0070] Considering the vehicle speed The update interval is small enough, so it can be considered that the autonomous driving vehicle c i Speed Remain constant. Definition represents the driving time of the autonomous driving vehicle in the kth broadcast cycle, is the two-dimensional (x-axis component, y-axis component) positioning estimate of the autonomous driving vehicle’s own geographic location in the k-1th broadcast cycle. is the random error of the autonomous vehicle's estimation of its own geographic location during the kth broadcast cycle. The autonomous vehicle's estimation of its own geographic location based on its local clock is:

[0071]

[0072] By analyzing the elements contained in formula (2), it can be seen that the autonomous driving vehicle c i Distance traveled during the kth broadcast cycle Is it affecting autonomous vehicles? i A major factor in estimating its own geographic location in the current k-th broadcast cycle. The period of CAM message broadcasting between the vehicle and the intelligent base station is related to the local clock of the vehicle. If there is a deviation between the local clock of the vehicle and the precise clock of the intelligent base station, there will inevitably be a deviation in the estimation of its own geographical location. Therefore, the time synchronization between the autonomous driving vehicle and the intelligent base station (precise clock) is the prerequisite for achieving accurate estimation of the geographical location of the autonomous driving vehicle.

[0073] This paper uses the periodic exchange of operating status information between the autonomous driving vehicle and the intelligent base station to iterate the local clock reading of the autonomous driving vehicle to obtain the same logical clock reading and achieve accurate positioning of the autonomous driving vehicle. For autonomous vehicles iThe specific expression of the logical clock reading in the kth broadcast cycle is as follows:

[0074]

[0075] in is the correction coefficient of the drift rate of the local clock of the autonomous driving vehicle during the kth broadcast cycle, It is the correction coefficient of the deviation value of the autonomous driving vehicle's local clock in the kth broadcast cycle.

[0076] Combining formulas (1), (2) and (3), the estimated geographic location of the autonomous vehicle can be summarized as:

[0077]

[0078] in Represents the estimated geographic location of the autonomous vehicle during the initial broadcast cycle, where for k≥2.

[0079] like Figure 1 As shown in the figure, a two-dimensional plane coordinate system is established with the intelligent base station as the origin and the autonomous driving vehicle c i Taking the kth interaction cycle after entering the intelligent base station broadcast coverage as an example, assume that the autonomous driving vehicle broadcasts a CAM message to the intelligent base station at position A, which contains the instantaneous speed of the current autonomous driving vehicle Local clock and the autonomous vehicle’s estimate of its own geographic location After receiving the CAM message broadcast by the autonomous vehicle, the intelligent base station will estimate the current geographical location of the autonomous vehicle based on its own state information, such as the local precise time, and attach it to the CAM message and broadcast it at the end of the kth broadcast cycle. Similar to formula (4), the intelligent base station estimates the current geographical location of the autonomous vehicle c i Current location estimates It can be expressed as:

[0080]

[0081] For any given value ε>0, if the autonomous driving vehicle and the intelligent base station can go through a finite number of interaction iterations (the number of interaction iterations is less than or equal to the bounded value ) and then meet:

[0082]

[0083] It can be considered that accurate positioning of the autonomous driving vehicle has been achieved.

[0084] 2.1、Asymmetric positioning synchronous communication link:

[0085] When the autonomous vehicle is moving, the distance from the intelligent base station changes dynamically, which will cause the autonomous vehicle to i The asymmetry between the transmitting link and the receiving link of the intelligent roadbed in the wireless communication process causes the positioning error to diverge. The demonstration process is as follows:

[0086] Define the error scaling factor For smart base stations and autonomous vehicles i The slope ratio of the positioning error in the kth and (k-1)th rounds of broadcast cycles, and when k→∞, At the same time, according to the definition of the error proportional factor, the error slope ratio can be written as:

[0087]

[0088] Formula (1) After substituting it into formula (7), we can get:

[0089]

[0090] Considering the autonomous vehicle i The transmission of information between the intelligent base station takes time, so formula (8) is modified appropriately:

[0091]

[0092] in, and are the autonomous driving vehicles c in the kth and (k-1th) broadcast cycles respectively. i The time interval between broadcasting a CAM message and receiving the CAM message by the intelligent base station, i.e., the time delay of the CAM message transmission, is the local clock reading.

[0093] Theoretically, if the positioning error between the autonomous vehicle and the smart base station is converged or bounded, then the error scaling factor Should always be less than or equal to the initial value However, due to the high-speed movement of vehicles, the transmission delays of sending and receiving signals in the cooperative positioning process are not symmetrical, that is, The error proportional factor in the kth broadcast cycle is Greater than its initial value That is, the positioning error between the vehicle and the intelligent base station oscillates and diverges. When k→∞, equation (9) can be expressed as:

[0094]

[0095] From the above special case, if the positioning error is convergent, it must satisfy The above conditions may not be met when the vehicle is moving. It can be seen that there will be asymmetric transmission and reception links between the vehicle and the intelligent roadbed during the wireless collaborative positioning process, which will cause asymmetric time delays in the process of sending and receiving CAM messages, causing oscillation and divergence of positioning errors.

[0096] 2.2 C-ATS algorithm:

[0097] Considering that the ATS algorithm cannot achieve accurate positioning in the presence of asymmetric communication delays, this paper proposes a geolocation synchronization algorithm suitable for asymmetric communication networks. The vehicle corrects its estimate of the current geographic location by calculating the deviation between the estimated geographic location and the intelligent base station. After multiple rounds of iterations, the vehicle's geographic location estimate is finally consistent. At the same time, considering that the error in vehicle speed estimation will affect the positioning accuracy of the vehicle-road collaborative positioning algorithm, this paper designs an adaptive Kalman filter algorithm to improve the accuracy of vehicle speed estimation, so that the C-ATS algorithm can overcome the asymmetric delay while improving positioning accuracy.

[0098] During the synchronization process, the vehicle c i Update its own local clock period Local clock update cycle with intelligent base station For comparison, use the filter scale factor To characterize the estimated ratio of the deviation between each other's local clocks after each round, where ρ η represents the weight coefficient. After that, vehicle c i The filtering ratio factor will be based on each broadcast cycle The value is based on the local logical clock The correction factor of the local logical clock is adjusted, including the drift rate correction factor of the local logical clock and deviation correction factor The specific algorithm is as follows, where ρ α , o The vehicle c i Correct the weight coefficient of the estimated location of the location.

[0099] The high-precision positioning algorithm for autonomous driving vehicles under roundabouts based on the time synchronization algorithm includes:

[0100] Step 1: Install an intelligent base station at the center of the roundabout. The intelligent base station can obtain accurate time information. The intelligent base station and the autonomous driving vehicle c i They can interact with each other through wireless communication in the form of periodic broadcasting;

[0101] Step 2: Autonomous driving vehicle c i In the first broadcast cycle, a CAM message is broadcast to the intelligent base station. The CAM message includes the current autonomous driving vehicle c i The local clock Instantaneous speed and autonomous vehicles i On local clock Geographic location valuation in, Can be obtained through GPS;

[0102] Step 3: The intelligent base station receives the autonomous driving vehicle c i After the CAM message is broadcast, the autonomous driving vehicle c is obtained from the CAM message i Current location estimates

[0103] Step 4: Autonomous driving vehicle c i By pre-setting the drift correction factor and offset correction factor Value versus time Make corrections and get Will Assign to

[0104] Step 5: Autonomous driving vehicle c i In the k≥2th broadcast cycle, a CAM message is broadcasted to the intelligent base station. The CAM information includes the current autonomous driving vehicle c i The local clock Instantaneous speed and autonomous vehicles i Valuation of your geographic location in:

[0105]

[0106] in, represents the autonomous vehicle c i The random error in estimating its own geographic location during the rth broadcast cycle;

[0107] Step 6: The intelligent base station receives the autonomous driving vehicle c i After broadcasting the CAM message, Time, for autonomous driving vehicles c i Estimate the current geographic location and get the estimated value in:

[0108]

[0109] in, It means that the intelligent base station sends a signal to the autonomous driving vehicle c in the rth broadcast cycle. i Random errors in geolocation estimates, Indicates the local clock of the intelligent base station;

[0110] Smart base stations will be valued It is broadcasted in the CAM message;

[0111] Step 7: Autonomous Driving Vehicle c i The time of receiving the CAM message broadcast by the intelligent base station is Autonomous driving vehicles i Judgment: For any given value ε>0, if The positioning is completed. For autonomous vehicles i On local clock The exact geographical location of Then execute step 8;

[0112] Step 8: Autonomous Driving Vehicle c i Calculate the filter scaling factor based on the C-ATS algorithm Filter scale factor for:

[0113]

[0114] Among them, ρ η is the weight coefficient of the filter proportional factor;

[0115] Calculate the drift correction factor by filtering the proportional factor and offset correction factor

[0116] Offset correction factor for:

[0117]

[0118] Among them, ρ o For autonomous vehicles i Correct the offset weight coefficient of the estimated own geographic location;

[0119] Drift correction factor for:

[0120]

[0121] Among them, ρ α For autonomous vehicles i Corrected the drift weight coefficient of the geographic location estimate;

[0122] Step 9: Using the Drift Correction Factor and offset correction factor For local clock Make corrections and get Will Assign to

[0123] Step 10: k=k+1; return to execute step 6.

[0124] The instantaneous speed or The method to obtain is:

[0125] Autonomous driving vehicles i The wheel speed sensor on the vehicle is used to obtain the instantaneous speed measurement value, and the adaptive Kalman filter algorithm is used to filter the instantaneous speed measurement value to obtain the updated instantaneous speed. or

[0126] 3. Vehicle speed estimation algorithm based on adaptive Kalman filter:

[0127] The vehicle-road collaborative positioning solution places high demands on the accuracy of vehicle speed. The higher the speed accuracy, the higher the positioning accuracy. However, the existing vehicle speed sensors have low measurement accuracy, and a filtering algorithm is needed to process the measured vehicle speed. The Kalman filter algorithm (KF) has become a commonly used algorithm for predicting and correcting target quantities and filtering noise because of its simple algorithm and wide application. However, the standard KF algorithm is too simple in describing and processing noise, resulting in limited filtering effects. Therefore, an adaptive Kalman filter algorithm (AKF) is designed to adaptively process the mean and variance of the measurement noise to improve the accuracy of vehicle speed estimation.

[0128] Taking the digital output signal of the wheel speed sensor as the measurement quantity, the system state space model is obtained as follows:

[0129]

[0130] Where X k-1 , X k is the state vector at the k-1th and kth moments, Y k is the wheel speed measurement value, W k-1 and V k are the process noise vector and the measurement noise vector, respectively, k and A are the measurement matrix and state transfer matrix respectively.

[0131] The state vector is X k =[v k ak ] T , the system process noise vector is System state transfer matrix A, wheel speed measurement matrix H k and the process noise covariance matrix Q k They are:

[0132]

[0133] where v k and a k are the longitudinal velocity and acceleration at the kth moment, Δt is the sampling time interval, and w k represents the process noise of vehicle speed and acceleration, δ v and δ v express and w k The variance of .

[0134] The core problem of the original Kalman filter algorithm is the description and processing of noise. Assume that the system process noise W k is zero mean white noise, then the measured noise V k The main reason for the speed error is the measurement noise V k The mean is not zero, so it needs to be transformed. Let E(V k )=u v ,make The system model is transformed into:

[0135]

[0136] in is zero-mean measurement noise.

[0137] According to the fluctuation of the measured value at the last moment and the estimated error at the last moment, the measurement noise mean u is adjusted in real time v and the measurement noise equivalent covariance R k The wheel speed measurement value Y at the kth, k-1th, ..., knth moment k-i And the vehicle reference speed v estimated by the Kalman filter algorithm k-i Perform linear regression and get Y k-i and v k-i The linear fitting formula and Y k-i The mean of the residual sum of squares is d.

[0138]

[0139] remember:

[0140] Δv=max(Y k -v k ,Yk-1 -v k-1 ,…,Y k-n -v k-n );

[0141]

[0142] R k =d;

[0143] When the Kalman filter recursive algorithm is used to determine the reference vehicle speed, the initial value is set as:

[0144]

[0145] in is the state vector at the initial moment, u0 is the wheel speed at the initial moment, P0 is the variance of the state vector at the initial moment, Q0 is the covariance matrix of the process noise vector at the initial moment, and R0 is the equivalent covariance of the measurement noise at the initial moment.

[0146] Determine v and R k After that, the original Kalman filter algorithm is applied to calculate the vehicle speed estimate, which is described by the following equations:

[0147]

[0148] Where P is the error covariance matrix and K is the Kalman filter gain.

[0149] 4. Simulation analysis:

[0150] In order to analyze and verify the influence of different collaborative positioning algorithms on the convergence and divergence of positioning errors under asymmetric link delay, Matlab is used to simulate and verify the collaborative positioning algorithm of vehicles in the roundabout scenario. The model is set up as a distributed network consisting of a fixed node (simulating intelligent roadbed) and a mobile node (simulating mobile vehicle). The nodes follow the relevant requirements of the international vehicle-mounted self-organizing network communication standard C-ITS and IEEE802.11p, and broadcast CAM messages with a period of 100 milliseconds, which contain information such as the node's real-time speed, local time, and estimated geographic location.

[0151] 4.1. Simulation analysis of time synchronization algorithm:

[0152] We selected the ATS algorithm, WMTS algorithm, LSTS algorithm and C-ATS algorithm, and first simulated and compared the synchronization performance of different time synchronization algorithms. The parameter settings are shown in Table 1.

[0153] Table 1, time synchronization algorithm parameter table:

[0154] ATS WMTS LSTS C-ATS <![CDATA[ρ η ]]> 0.5 1 / (1+k) <![CDATA[1 / (1+…+k 2 )]]> 1 / k <![CDATA[ρ α ]]> 0.5 1 <![CDATA[1 / (1+k) 0.6 ]]> 1 / k <![CDATA[ρ o ]]> 0.5 1 0.4 0.5

[0155] Taking into account various factors such as the vehicle's manufacturing process and environmental impact, the vehicle is set to move at a constant speed of 60 km / h, and the vehicle's clock drift slope α i =0.9989, clock offset deviation o i =1, and select the random communication delay in the interval (0,0.1) and obey the uniform distribution for the test. The ATS and WMTS algorithms are used to simulate the vehicle for 50s, and the LSTS and C-ATS algorithms are used to simulate the vehicle for 200s. The simulation results are as follows.

[0156] Depend on Figure 2 It can be seen that the ATS algorithm and the WMTS algorithm cannot achieve time synchronization in the presence of asymmetric random communication time delay, and the time deviation cannot converge. After the simulation starts, the time deviation of the ATS algorithm gradually diverges and increases, and the maximum time deviation within 50s is 9.48s. For the WMTS algorithm, the time deviation decreases rapidly after the simulation starts, and then remains in the range of (-110ms, 110ms).

[0157] Depend on Figure 3 It can be seen that the LSTS algorithm and the C-ATS algorithm can achieve time synchronization in the presence of asymmetric random communication time delay, and the time deviation does not diverge. Both the LSTS algorithm and the C-ATS algorithm converge quickly after the start of the simulation. The LSTS algorithm converges after about 40 interactions, and the positioning deviation stabilizes at 14.2ms. The positioning deviation of the C-ATS algorithm decreases rapidly within 20 interactions, and there is a small vibration within 300 interactions, but the maximum positioning deviation remains within 2.3ms, and then the positioning deviation converges to 1.2ms. It can be seen that the C-ATS algorithm can achieve the convergence of the synchronization time deviation compared with the ATS algorithm and the WMTS algorithm, and in the most ideal case in the simulation environment set in this article, the time synchronization accuracy is improved by 13ms compared with the LSTS algorithm, and the time deviation is reduced by 91.5%.

[0158] 4.2. Simulation analysis of adaptive Kalman filtering algorithm:

[0159] In order to analyze and verify the performance of the adaptive Kalman filter algorithm, this paper selects the standard Kalman filter algorithm and the adaptive Kalman algorithm for comparison. In order to simulate the actual road conditions and vehicle operation, we select the vehicle speed under the six standard working conditions of the vehicle cycle as the standard vehicle speed. The standard vehicle speed change process is as follows: Figure 4 As shown by the black broken line in the middle, one operation cycle is 96.1s, including six operating conditions: uniform speed-acceleration-uniform speed-acceleration-uniform speed-deceleration.

[0160] Taking many sensors such as NXP Semiconductors SAF775x, Nidec E-Axle and Bosch 0 280 218 190 as examples, the detection error indicated in their data sheets is 5%, such as Figure 4 The orange curve in Figure 2 shows the

[0161] Will Figure 4 The simulation experiment of the measured vehicle speed input uniformly distributed within the range of ±5% of the standard vehicle speed shown in the figure, the simulation results after being processed by the standard Kalman filter algorithm and the adaptive Kalman filter algorithm are shown in Figure 5 shown.

[0162] The speed error of the standard Kalman filter algorithm decreases rapidly within 50 interactions, and fluctuates around 5 cm / s in the first uniform speed driving condition, fluctuates around 1.5 cm / s in the first accelerated driving condition, and then fluctuates within the range of (-0.57 cm / s, 0.42 cm / s). The speed error of the adaptive Kalman filter algorithm also decreases rapidly within 50 interactions, fluctuates around 2 cm / s in the first uniform speed driving condition, fluctuates around -0.6 cm / s in the first accelerated driving condition, and then converges from the second uniform speed driving condition, keeping the speed error at 0.06 cm / s.

[0163] Compared with the standard Kalman filtering algorithm, the adaptive Kalman filtering algorithm not only achieves the convergence of vehicle speed error, but also Figure 5 Under the working conditions shown, the vehicle speed accuracy is improved by 0.51cm / s. Under the most ideal conditions, the vehicle speed error can be reduced by 89.5%.

[0164] 4.3 Performance analysis of positioning algorithm based on C-ATS and adaptive Kalman filtering:

[0165] 4.3.1. Impact of adaptive Kalman filter algorithm on positioning:

[0166] We combined the C-ATS algorithm with superior time synchronization performance and vehicle speed filtering accuracy with the adaptive Kalman filter algorithm to conduct a vehicle-road collaborative positioning simulation experiment. In order to achieve a fair comparison of the positioning performance of the algorithm, four indicators, namely single positioning deviation, cumulative positioning deviation, positioning deviation within every 10 seconds, and cumulative absolute positioning deviation, were selected to analyze the three situations before filtering, after using the standard Kalman filter, and after using the adaptive Kalman filter. The meaning of each indicator is as follows:

[0167] Single positioning deviation The difference between the vehicle's location after the kth vehicle-road collaborative positioning and the actual vehicle's location. After each interaction between the vehicle and the base station, a positioning synchronization is performed, that is, the positioning update cycle is 100ms. It can be expressed as follows:

[0168]

[0169] Cumulative positioning deviation The algebraic sum of the differences between the vehicle's location after the first k vehicle-road collaborative positioning and the actual vehicle's location, and the vehicle and the base station do not synchronize their positioning after each interaction, that is, the positioning update cycle is 96.1s. It can be expressed as follows:

[0170]

[0171] Positioning deviation within every 10 seconds The algebraic sum of the differences between the vehicle's location after the first k vehicle-road collaborative positioning and the actual vehicle's location, and the vehicle and the base station perform positioning synchronization once every 100 interactions, that is, the positioning update cycle is 100ms. It can be expressed as follows:

[0172]

[0173] Where m=k MOD 100.

[0174] Cumulative absolute positioning deviation The absolute value of the difference between the vehicle's location after the first k vehicle-road collaborative positioning and the actual vehicle's location is the sum, and the vehicle and the base station do not synchronize their positioning after each interaction, that is, the positioning update cycle is 96.1s. It can be expressed as follows:

[0175]

[0176] The single, cumulative, every 10s and cumulative absolute positioning deviations before filtering, after vehicle speed processing by the standard Kalman algorithm and after vehicle speed processing by the adaptive Kalman algorithm are compared horizontally, as shown in Figure 2. Figure 6 , 7 , 8 and 9.

[0177] Figure 6It can be seen that the maximum single positioning deviation before filtering is 8.3cm; after the standard Kalman algorithm is used for speed processing, the single positioning deviation gradually converges to about 0.5cm during the acceleration and deceleration stage of the vehicle, and converges to 0.2cm during the constant speed stage; after the adaptive Kalman algorithm is used for speed processing, it is not affected by the change of vehicle speed, and gradually converges to 0.02cm after 120 interactions. In the most ideal case, compared with before filtering, the single positioning deviation can be reduced by 99.7%, and compared with the standard Kalman filtering, the single positioning deviation can be reduced by 90% to 96%.

[0178] Figure 7 It can be seen that before filtering, the maximum positioning deviation within 10 seconds is 84.4 cm when there are 660 interactions; after the standard Kalman algorithm is used for vehicle speed processing, the maximum positioning deviation within 10 seconds is 21.6 cm after 500 interactions; after the adaptive Kalman algorithm is used for vehicle speed processing, the positioning deviation within 10 seconds converges after 251 interactions, and the maximum value is 0.42 cm. In the most ideal case, it is reduced by 99.5% compared with before filtering and 98.1% compared with standard Kalman filtering.

[0179] Figure 8 It can be seen that the cumulative positioning deviation fluctuates greatly before filtering, with the maximum absolute value of 136.5 cm at 658 interactions; after the standard Kalman algorithm is used for vehicle speed processing, the fluctuation of the cumulative positioning deviation is reduced, with the maximum value of 54.6 cm at 746 interactions; after the adaptive Kalman algorithm is used for vehicle speed processing, the cumulative positioning deviation converges after 251 interactions, with a maximum value of 17.8 cm. In the most ideal case, it can be reduced by 86.9% compared with before filtering and by 67.4% compared with standard Kalman filtering.

[0180] Fig. 9 It can be seen that the cumulative absolute positioning deviation of the vehicle before filtering diverges rapidly, and is almost proportional to the number of interactions. At the end of the cycle, the positioning deviation reaches 3630cm; after the standard Kalman algorithm is used to process the vehicle speed, the increase in the cumulative absolute positioning deviation of the vehicle slows down. At the end of the cycle, the positioning deviation is 403cm; after the adaptive Kalman algorithm is used to process the vehicle speed, the cumulative absolute positioning deviation of the vehicle increases to 223cm after the 10th interaction, and then begins to increase slowly. It gradually converges after 260 interactions, and at the end of the cycle, the positioning deviation is 259cm. In the most ideal case, it can be reduced by 92.9% compared with before filtering and by 35.7% compared with the standard Kalman filter.

[0181] The single, cumulative, every 10s and cumulative absolute positioning deviations before filtering, after vehicle speed processing by the standard Kalman algorithm and after vehicle speed processing by the adaptive Kalman algorithm are compared longitudinally, as shown in Figure 2. Fig.10 , 11 and 12.

[0182] It can be seen that the vehicle's single positioning deviation, cumulative positioning deviation and positioning deviation within every 10 seconds before filtering oscillate continuously within the interaction cycle due to the randomness of the vehicle speed error; in addition, the positioning synchronization every 10 seconds plays a role in limiting the divergence of the positioning deviation within every 10 seconds.

[0183] 4.3.2 Impact of C-ATS algorithm on positioning:

[0184] To further compare the performance of C-ATS and LSTS algorithms in vehicle-road collaborative positioning, we selected four cases: precise vehicle speed, 1% random vehicle speed error, 1% random vehicle speed error after adaptive Kalman filtering, and 5% random vehicle speed error after adaptive Kalman filtering. The cumulative positioning deviations of the vehicles in the four cases are as follows: Fig.13 , 14 , 15 and 16.

[0185] Depend on Fig.13 It can be seen that in the simulation environment set up in this paper, the LSTS algorithm can make the positioning deviation converge to 2.11cm under the precise vehicle speed, and the C-ATS algorithm can make the positioning deviation converge to 0.25cm. In the most ideal case, compared with the LSTS algorithm, the positioning accuracy is improved by 1.86cm and the positioning deviation is reduced by 88.2%. Fig.14 It can be seen that under a 1% random vehicle speed error, the maximum positioning deviation is 42.47cm when using the LSTS algorithm, and the maximum value is 39.22cm when using the C-ATS algorithm under the most ideal conditions. Compared with the LSTS algorithm, the positioning accuracy is improved by 3.25cm and the positioning deviation is reduced by 7.65%.

[0186] Depend on Fig.15 It can be seen that after the 1% random speed error is filtered by adaptive Kalman, the positioning deviation is 3.08cm at the end of the cycle using the LSTS algorithm, and 0.72cm at the end of the cycle using the C-ATS algorithm. Compared with the LSTS algorithm, the positioning accuracy is improved by 2.36cm and the positioning deviation is reduced by 76.6%. Fig.16It can be seen that after the 5% random speed error is adaptively filtered by Kalman, the positioning deviation is 18.35cm at the end of the cycle using the LSTS algorithm and 16.54cm using the C-ATS algorithm. In the most ideal case, the positioning accuracy can be improved by 1.81cm and the positioning deviation can be reduced by 9.86% compared with the LSTS algorithm.

[0187] It can be seen from the above figure that the positioning accuracy when using the C-ATS algorithm in vehicle collaborative positioning is better than that of the LSTS algorithm. The degree of improvement in positioning accuracy is proportional to the vehicle speed accuracy. The lower the vehicle speed accuracy, the lower the degree of improvement in positioning accuracy. When there is no vehicle speed error, the degree of improvement in positioning accuracy is ideally up to 88.2%.

[0188] The protection scope of the present invention includes but is not limited to the above embodiments. The protection scope of the present invention shall be based on the claims. Any replacement, deformation, and improvement of the technology that can be easily thought of by technicians in this field shall fall within the protection scope of the present invention.

Claims

1. A high-precision positioning algorithm for autonomous driving vehicles under roundabouts based on a time synchronization algorithm, characterized in that: include: Step 1: Install an intelligent base station at the center of the roundabout. The intelligent base station can obtain accurate time information. The intelligent base station and the autonomous driving vehicle c i They can interact with each other through wireless communication in the form of periodic broadcasting; Step 2: Autonomous driving vehicle c i In the first broadcast cycle, a CAM message is broadcast to the intelligent base station. The CAM message includes the current autonomous driving vehicle c i The local clock Instantaneous speed and autonomous vehicles i On local clock Geographic location valuation Step 3: The intelligent base station receives the autonomous driving vehicle c i After the CAM message is broadcast, the autonomous driving vehicle c is obtained from the CAM message i Current location estimates Step 4: Autonomous driving vehicle c i By pre-setting the drift correction factor and offset correction factor Value versus time Make corrections and get Will Assign to Step 5: Autonomous driving vehicle c i In the k≥2th broadcast cycle, a CAM message is broadcasted to the intelligent base station. The CAM information includes the current autonomous driving vehicle c i The local clock Instantaneous speed and autonomous vehicles i Valuation of your geographic location in: in, represents the autonomous vehicle c i The random error in estimating its own geographic location during the rth broadcast cycle; Step 6: The intelligent base station receives the autonomous driving vehicle c i After broadcasting the CAM message, Time, for autonomous driving vehicles c i Estimate the current geographic location and get the estimated value in: in, It means that the intelligent base station sends a signal to the autonomous driving vehicle c in the rth broadcast cycle. i Random errors in geolocation estimates, Indicates the local clock of the intelligent base station; Smart base stations will be valued It is broadcasted in the CAM message; Step 7: Autonomous Driving Vehicle c i The time of receiving the CAM message broadcast by the intelligent base station is Autonomous driving vehicles i Judgment: For any given value ε>0, if The positioning is completed. For autonomous vehicles i On local clock The exact geographical location of Then execute step 8; Step 8: Autonomous Driving Vehicle c i The filter proportional factor is calculated according to the C-ATS algorithm, and the drift correction coefficient is calculated through the filter proportional factor and offset correction factor Filter scale factor for: Among them, ρ η is the weight coefficient of the filter proportional factor; Offset correction factor for: Among them, ρ o For autonomous vehicles i Correct the offset weight coefficient of the estimated own geographic location; Drift correction factor for: Among them, ρ α For autonomous vehicles i Corrected the drift weight coefficient of the geographic location estimate; Step 9: Using the Drift Correction Factor and offset correction factor For local clock Make corrections and get Will Assign to Step 10: k=k+1; return to execute step 6.

2. The high-precision positioning algorithm for autonomous driving vehicles under roundabouts based on time synchronization algorithm according to claim 1 is characterized in that: The instantaneous speed or The method to obtain is: Autonomous driving vehicles i The wheel speed sensor on the vehicle is used to obtain the instantaneous speed measurement value, and the adaptive Kalman filter algorithm is used to filter the instantaneous speed measurement value to obtain the updated instantaneous speed. or 3. The high-precision positioning algorithm for autonomous driving vehicles under roundabouts based on time synchronization algorithm according to claim 1 is characterized in that: In step 4:

4. The high-precision positioning algorithm for autonomous driving vehicles under roundabouts based on time synchronization algorithm according to claim 1 is characterized in that: Through the drift correction factor and offset correction factor For local clock Make corrections and get Specifically:

Images