A multi-sensor multi-vehicle cooperative positioning system and method

Abstract

The application provides a multi-sensor multi-vehicle cooperative positioning system and method, and relates to the technical field of vehicle positioning; the system comprises a GPS receiving module, an IMU module, an in-vehicle data acquisition module, an out-vehicle data acquisition module, a reinforcement learning module and a filtering fusion module; the GPS receiving module is used for receiving satellite signals; the IMU module is used for obtaining angular velocity, speed and acceleration estimation of the vehicle; the in-vehicle data acquisition module is used for acquiring wheel speed data, steering angle and mileage data of the vehicle; the out-vehicle data acquisition module is used for obtaining relative position and speed estimation relative to other cooperative vehicles; the reinforcement learning module is used for improving the positioning accuracy of the GPS device; the filtering fusion module fuses various information to obtain accurate position information of the vehicle; the application fuses information from local vehicle-mounted sensors and observations of other vehicles or existing intelligent transportation system infrastructure (such as road side units) to improve the accuracy and stability of positioning and movement tracking.

Description

Technical Field

[0001] This invention relates to the field of vehicle positioning technology, and in particular to a multi-sensor, multi-vehicle collaborative positioning system and method. Background Technology

[0002] Vehicle localization is a fundamental task of autonomous driving, providing essential information to guide the operation of Intelligent Transportation Systems (ITS), such as determining the accurate location of multiple vehicles in collaborative scenarios. In recent years, many researchers have been trying to develop new technologies to provide accurate vehicle localization information, including early, simple, and economical dead reckoning techniques, the currently popular GPS-based techniques, and more recent techniques based on markers and high-precision maps. GPS localization is susceptible to environmental factors and cannot provide the required accuracy in certain situations (such as congested cities, under bridges, near tall buildings, tunnels, etc.). While map-based localization techniques can provide more accurate positioning, they rely on high-precision maps, which incurs additional high costs, making them not the optimal choice for most ITS applications.

[0003] To address the limitations of single-sensor-based technologies, researchers have attempted to use information from multiple sensors for localization and motion tracking to achieve higher accuracy. Within this framework, traditional filtering algorithms, such as particle filters and Kalman filters, are applied to fuse measurements from different sensors and their prior information to obtain more accurate estimates. However, in existing literature, most solutions can only fuse and utilize information from different sensors when all sensors are located on the same vehicle. Furthermore, despite the involvement of various sensors, accuracy remains heavily reliant on GPS and is highly sensitive to environmental conditions. In summary, existing single-vehicle multi-sensor strategies still cannot address key challenges, such as the potential for signal congestion in dense traffic environments, leading to a significant reduction in vehicle localization accuracy and impacting driving safety. To overcome these limitations, some researchers have proposed solutions that leverage information from other vehicles to enhance the stability of single-vehicle sensors. However, most existing solutions rely on traditional localization sensors, thus requiring the construction of additional supporting infrastructure within intelligent transportation systems to assist the localization process. Moreover, these traditional localization sensor-based technologies assume a static vehicle model, thus hindering effective motion tracking. In other words, they can only locate results based on a single snapshot observation report at each point in time, and cannot take advantage of the temporal relationships between observations over a period of time.

[0004] For the vision of future "connected vehicles," a crucial task is to enable collaboration between different vehicles and combine sensor information from these vehicles to improve positioning accuracy. The most common technology is vehicle-to-vehicle (V2V) multi-base station technology, where vehicles transmit their own positions and calculate distances to other vehicles to obtain a more accurate self-localization estimate. However, most multi-base station-based research only uses GPS, IMU, and V2V wireless communication modules, failing to utilize sensors from other vehicles such as radar and lidar. In summary, there is currently no universal framework for combining information from traditional positioning devices with more advanced sensing equipment to improve the positioning accuracy of intelligent connected vehicles. Summary of the Invention

[0005] To address the shortcomings of existing technologies, this invention provides a multi-sensor, multi-vehicle cooperative positioning system and method.

[0006] A multi-sensor, multi-vehicle cooperative positioning system includes a signal acquisition device, a central processing unit, and a communication device. The signal acquisition device is responsible for acquiring the location information of the vehicles, the communication device is responsible for the interaction of signals acquired by each vehicle, and the central processing unit is responsible for fusing and filtering the signals from its own signal acquisition device and communication device to obtain the accurate location information of the vehicle itself.

[0007] The signal acquisition device includes a GPS receiving module, an IMU module, an in-vehicle data acquisition module, and an external data acquisition module; the central processing unit includes a reinforcement learning module, a self-localization filtering and fusion module, a relative localization filtering and fusion module, and a global localization filtering and fusion module.

[0008] The GPS receiving module is used to receive satellite signals, send the received signals to the reinforcement learning module, and finally send them to the self-localization filtering and fusion module.

[0009] The IMU module is used to obtain the vehicle's angular velocity, velocity, and acceleration estimates, and sends the measured data to the self-localization filtering and fusion module;

[0010] The in-vehicle data acquisition module is used to collect the vehicle's wheel speed data, steering angle and mileage data, and input these data as input variables into the self-localization filtering fusion module;

[0011] The external data acquisition module includes a camera and radar, used to obtain relative position and speed estimates relative to other cooperating vehicles;

[0012] The reinforcement learning module is used to improve the positioning accuracy of GPS so as to more accurately locate the vehicle in the case of autonomous or semi-autonomous driving; the goal is to find the optimal correction strategy for the observed GPS longitude and latitude coordinates to generate more accurate location information.

[0013] The self-localization filtering and fusion module obtains the vehicle's accurate position information by filtering and fusing information from the reinforcement learning module, IMU module, in-vehicle data acquisition module, and external data acquisition module.

[0014] The relative positioning filtering and fusion module obtains the position information of other cooperating vehicles by filtering and fusing the vehicle's own position information obtained by the self-positioning fusion filtering module with the relative position and speed estimates relative to other cooperating vehicles obtained from the external data acquisition module.

[0015] The global positioning filtering and fusion module obtains more accurate global vehicle position information by filtering and fusing the vehicle's own position information with the relative position estimation information from other cooperating vehicles.

[0016] The communication device is used to realize communication between intelligent vehicles or communication between intelligent vehicles and roadside units (RSUs) in existing intelligent transportation systems.

[0017] A multi-sensor, multi-vehicle cooperative localization method, based on the aforementioned multi-sensor, multi-vehicle cooperative localization system, includes the following steps:

[0018] Step 1: The GPS receiving module acquires, tracks, synchronizes, and frames satellites and performs positioning calculations to obtain the longitude, latitude, and elevation of the vehicle's location. The IMU module obtains estimates of the vehicle's angular velocity, velocity, and acceleration. The in-vehicle data acquisition module collects wheel speed data, steering angle, and mileage data. Simultaneously, the external data acquisition module obtains estimates of the vehicle's relative position and velocity compared to other cooperating vehicles.

[0019] Step 2: The reinforcement learning module takes the longitude and latitude output of the GPS receiver as input and performs correction operations on the estimated longitude and latitude to provide a more accurate location output.

[0020] When a new data point is received, the reinforcement learning model trains an agent to determine the number of “units” that need to be adjusted for the observed longitude and latitude to return a more accurate location; this sequential decision problem is modeled as a partially observable Markov decision process (POMDP); the model’s goal is to learn a policy π(a|z,θ), where a represents the action vector, z represents the observation vector, and θ represents the model parameter vector; the goal of the policy is to parameterize the conditional probability of performing action a given observation z to maximize its reward.

[0021] The following introduces the proposed reinforcement learning model: 1) Action space: Actions are defined as longitude-latitude update operations; to reduce computational complexity, continuous longitude and latitude values ​​are discretized into small step sizes;

[0022] 2) Observation and Model Input: GPS devices report their locations at a certain frequency; in the proposed reinforcement learning model, an observation is not limited to the last reported GPS location, but is a stacked vector containing the last reported location and the history of the most recently predicted point; instead of using the reported GPS trajectory, model predictions are used to form the observation history vector; the prediction frequency is set to a value higher than the GPS data collection frequency; by forming the observation vector in this way, the model utilizes the historical trajectory information of the GPS device and the performance of the model to learn a high-quality policy to correct the reported GPS points;

[0023] Let the GPS reporting point at time t be denoted as q. t Its actual position is represented as g t Since the true location is unknown, the problem is formulated as a partially observable Markov decision process (POMDP); in this POMDP, p... t Indicates GPS reporting point q t The confidence state, which is represented as p in the RL model. t Some observable states are replaced by their estimates, i.e., confidence states, to form an MDP; an observation buffer of size N is used. t To store the historical model estimates of the most recent N-1 GPS reporting points and the current q t That is, Z t ={p t-N-1 ,…,p t-1 ,q t}; Using S t and b t Z t The hidden state and confidence state; given an observation buffer of size N, at time t, vector S t The corresponding real location buffer containing these points, i.e., S t ={g t-N,…,g t Vector b t The estimate includes the most recent N points, i.e., b t ={p t-N ,…,p t};Z t and b t Z differs only in its last element. t The last element is q t And vector b t The last element is p t The model is based on Z. t Estimate b t ;

[0024] Based on the POMDP setup described above, the goal of the reinforcement learning agent at each timett is to find the optimal correction action to correct q. t This process is based on a sliding window; once a new q is received... t The sliding window moves forward one step, forming a new observation vector of size N, where q t The final element, the last N-1 beliefs, constitute the observation vector Z. t The rest;

[0025] Whenever the GPS device reports a new location q t When the model is trained, it moves to the next observation buffer; when p is obtained via GPS device t At that time, it is pushed into the observation buffer to replace q. t Meanwhile, the observations are moved to time t+1; in each training step, the reinforcement learning model’s observations include observed GPS points and a series of historical estimates.

[0026] 3) Confidence Reward: Assuming the noise from GPS observations follows a white Gaussian distribution, the confidence ellipse reflects the uncertainty of the model's predictions. A smaller confidence ellipse indicates confidence in the model's predictions; otherwise, a larger area indicates confidence. When a new GPS point is observed at time t, the model is trained k times, not just once, based on the model parameters obtained at time t-1 and the new observation input, yielding k possible output predictions. Two metrics are used to measure the impact of uncertainty on the reinforcement learning model's performance. Both metrics are based on the covariance matrix of the surrogate's predictions. This covariance matrix is ​​a two-dimensional matrix representing the certainty of the direction of the correction action for the longitude and latitude of the GPS observations. Let the eigenvalues ​​of this covariance matrix be a and b. If both a and b are sufficiently different from zero, their product is used to calculate the area of ​​the confidence ellipse, with the formula πab. Otherwise, if at least one of these values ​​is close to zero, a+b is used as the surrogate measure of prediction confidence. A higher confidence measure indicates lower uncertainty. The reward function is constructed using the concept of uncertainty.

[0027] Although maximizing the confidence measure minimizes the predicted covariance, the predicted location may not necessarily be within the road constraints. Therefore, this model utilizes digital map information to improve predictions, incorporating map matching into the reward function. Map matching algorithms use a matching process to map observed data points (x, y) onto a road; thus, they are treated as a simple search problem. Map matching may be inaccurate when the road network is too complex or the observed data points deviate significantly from their true locations on the ground. Therefore, map matching is included as a regularization term only in the proposed model. Assuming the map matching result for the observation set (X, Y) is... Define the reward for one step as:

[0028] r=z+γ×D

[0029] Where Z is the confidence measure, γ is the regularization parameter, and D is the sum of the negative squared errors between the observed values ​​and the map matching results; that is...

[0030] Given a strategy π, define τ = {a0, O0, r1, a1, O1, r2, ..., a T-1 O T-1 ,r T-1 Let τ be a trajectory of the POMDP, and return the total reward on trajectory τ as the final reward of the policy; using a discount factor η, the reward function is expressed as:

[0031]

[0032] The goal of the model is to learn a policy that maximizes the discount reward return value R in each prediction. τ Set T=8 to pass Rt To approximate R τ Perform "n-step rewards";

[0033] 4) A3C Training Architecture: The A3C training architecture used requires shorter training sessions and provides more robust policies. A3C runs agents in parallel on multiple threads; it provides a diverse training environment for agents by allowing each agent to retain a copy of its state or observations and train an independent model.

[0034] Step 3: The self-localization filtering and fusion module takes the outputs of the reinforcement learning module, IMU module, and in-vehicle data acquisition module as inputs, and uses a Kalman filter (KF) to perform filtering and fusion to obtain the vehicle's own position information.

[0035] Setting the state variables of the Kalman filter KF

[0036] Where, x i y i These are the coordinates of vehicle i in the Cartesian coordinate system. Let be the speed of vehicle i; then the state transition equation and measurement equation of the Kalman filter KF are as follows:

[0037] x[k]=f(x[k-1],u[k],ω[k])

[0038] z[k] = g(x[k], v[k])

[0039] Where k is a discrete-time index, x is the vehicle's state including position and velocity, u is the command process, equivalent to driving input, representing acceleration, ω is command noise or state noise, which comes from the uncertainty of the command process; z is the measurement data reported by various sensors such as IMU, GPS, Radar, and camera; v is the data noise in the measurement and transmission process; f and g are the state equation and measurement model obtained from the physical dynamics of motion and the inherent characteristics of the sensing device, respectively.

[0040] The Kalman filter (KF) for filtering and fusion includes the following two steps:

[0041] Step S1: Time Update: Calculate the prior state and state transition Jacobian matrix to evaluate the prediction covariance;

[0042] Step S2: Measurement Update: Calculate the observation Jacobian matrix and Kalman filter gain;

[0043] Step 4: The relative positioning filtering and fusion module takes the output of the self-positioning filtering and fusion module and the relative position and speed estimates relative to other cooperating vehicles obtained from signal acquisition as input, and uses a Kalman filter (KF) for filtering and fusion to obtain the position information of other cooperating vehicles.

[0044] Step 5: Through communication devices, intelligent connected vehicles communicate with each other, exchanging the location information between cooperating vehicles obtained in Step 4. Furthermore, if a Remote Unit (RSU) exists within the intelligent transportation system, the RSU will be identified during vehicle localization, and the relative position from the RSU to the vehicle will be obtained. Simultaneously, the RSU will broadcast its absolute position coordinates to vehicles within its communication range. By calculating this information, the vehicle obtains its own absolute position estimate. Since the RSU's position information is reliable and accurate, with errors originating only from the vehicle's sensing devices, incorporating the RSU into the cooperation improves localization accuracy.

[0045] Step 6: The global filtering and fusion module takes the output of the self-localization filtering and fusion module in Step 3 and the position estimates of itself from other cooperating vehicles or RSUs in Step 5 as inputs, and uses the Kalman filter (KF) to perform filtering and fusion, resulting in a more accurate position estimate of the vehicle itself.

[0046] The goal of global filtering is to calculate the vehicle V based on the output of the local filter. S Optimal position estimation for vehicle V; S Self-estimation of state It is represented, and includes the covariance matrix P. S And from vehicle V i The local estimate is then used It is represented, and includes the covariance matrix P. i The local filtering results are also Gaussian distributed; therefore, the global optimal state estimate is expressed as a linear combination of the local estimates.

[0047] Under the above assumptions, global estimation Recorded as:

[0048]

[0049] Where A i and A s Let A be the unknown weights of the linear combination to be solved. i Weights estimated for other vehicles, A s These are the self-estimated weights; The variance is:

[0050]

[0051] To ensure the unbiasedness of the global estimate, the mean of the estimate cannot be changed; therefore, Ai Constraints:

[0052]

[0053] Based on the Gaussian assumption above, The maximum likelihood estimate is the estimate that minimizes the variance; therefore, global filtering becomes an optimization problem.

[0054]

[0055]

[0056] The Lagrange multiplier method is used to solve the convex optimization problem, thus obtaining the objective function:

[0057]

[0058] Finally, the optimal weights for the linear combination are obtained at the global filter:

[0059]

[0060]

[0061] The weights are inversely proportional to the local filtering performance;

[0062] Therefore, the final result of the global optimal estimate is:

[0063]

[0064] For a vehicle that senses and communicates with the RSU, considering the measurement of the RSU, the globally optimal estimate is expressed as:

[0065]

[0066] in:

[0067]

[0068]

[0069]

[0070] Because RSUs possess reliable and accurate location information, they improve the positioning accuracy of vehicles within their communication range. On the other hand, due to cooperation between vehicles, vehicles that receive assistance from RSUs further assist vehicles that cannot directly perceive or communicate with RSUs, thereby improving their positioning accuracy and thus enhancing the positioning and tracking performance of vehicles throughout the network.

[0071] Beneficial technical effects of the present invention:

[0072] This invention provides a multi-sensor multi-vehicle (MSMV) localization framework. The proposed framework features a novel two-layer structure, including global filtering and local filtering. A reinforcement learning framework is proposed in the local filters to improve GPS positioning accuracy. On one hand, traditional IMU and GPS data are used to obtain an estimate of the vehicle's own state. On the other hand, the vehicle observes other vehicles and obtains their states by generating sensing data through another set of local filters using an integrated sensing system. Then, the local estimates related to the same target vehicle from all vehicles are input into the global filter to obtain a global estimate of the target, thereby significantly improving the robustness and accuracy of the estimation. The localization process is guided by the vehicle's dynamic model, thus enabling motion tracking. The proposed MSMV framework is general, applicable not only to various types of sensors but also allowing for the implementation of local filters using different techniques to leverage dynamic motion models. Furthermore, the framework can incorporate intelligent infrastructure, such as roadside units (RSUs), during cooperative localization and motion tracking to further enhance performance. Attached Figure Description

[0073] Figure 1 This is a schematic diagram of the structure of a multi-sensor, multi-vehicle cooperative positioning system provided in an embodiment of the present invention;

[0074] Figure 2 A schematic diagram of the GPS accuracy enhancement reinforcement learning module provided in an embodiment of the present invention;

[0075] Figure 3 This is a flowchart of a multi-sensor, multi-vehicle cooperative positioning method provided in an embodiment of the present invention. Detailed Implementation

[0076] The specific embodiments of the present invention will be described in further detail below with reference to the accompanying drawings and examples.

[0077] In this example, the vehicle to be located is denoted as V. S It can autonomously estimate its own state through its own onboard sensors, and in the vehicle network, there are also N other vehicles cooperating, which can monitor V... S Observations are conducted to measure its state. Based on V S The vehicle's own sensor data, such as that from commonly used traditional positioning devices like IMUs and GPS, can be used to obtain a local estimate of its own state through local filtering. At the same time, each cooperating vehicle can also observe V. S And obtain the V through a local filter S State estimation These estimates are based on measurements of V obtained through sensing devices. SThe relative state between themselves and themselves. Combined with an estimate of their own positioning. They can obtain V S State estimation The independent estimation results of these local filters can be compared with V S Share, and then through V S Global filtering is performed at the point where all local estimates are fused to obtain x. S A global estimate. The same conditions apply to all vehicles, forming a decentralized framework.

[0078] A multi-sensor, multi-vehicle cooperative positioning system, such as Figure 1 As shown, it includes a signal acquisition device, a central processing unit, and a communication device; wherein, the signal acquisition device is responsible for acquiring the vehicle's location information, the communication device is responsible for the interaction of signals acquired by each vehicle, and the central processing unit is responsible for fusing and filtering the signals from its own signal acquisition device and communication device to obtain the vehicle's accurate location information.

[0079] The signal acquisition device includes a GPS receiving module, an IMU module, an in-vehicle data acquisition module, and an external data acquisition module; the central processing unit includes a reinforcement learning module, a self-localization filtering and fusion module, a relative localization filtering and fusion module, and a global localization filtering and fusion module.

[0080] The GPS receiving module is used to receive satellite signals, send the received signals to the reinforcement learning module, and finally send them to the self-localization filtering and fusion module.

[0081] The IMU module is used to obtain the vehicle's angular velocity, velocity, and acceleration estimates, and sends the measured data to the self-localization filtering and fusion module;

[0082] The in-vehicle data acquisition module is used to collect the vehicle's wheel speed data, steering angle and mileage data, and input these data as input variables into the self-localization filtering fusion module;

[0083] The external data acquisition module includes a camera and radar, used to obtain relative position and speed estimates relative to other cooperating vehicles;

[0084] The reinforcement learning module is used to improve the positioning accuracy of GPS devices so as to more accurately locate vehicles in autonomous or semi-autonomous driving scenarios. The goal is to find the optimal correction strategy for the observed GPS longitude and latitude coordinates to generate more accurate location information. The whole process is similar to a filtering-reinforcement learning (RL) model that takes the real-time longitude and latitude coordinates collected by the GPS device as input and uses the model to improve positioning. The output of the model is an action strategy on how to correct the observed data to generate a more accurate location.

[0085] In a typical GPS data stream, a GPS device receives signals from multiple satellites, each signal containing the satellite's position and the signal's transmission time. The GPS device can locate itself based on the satellite's position at the time of signal transmission and the signal's propagation time. This process is as follows: Figure 2 As shown in (a). For accurate positioning, a GPS receiver needs to receive high-quality signals with negligible latency from at least four satellites. While the receiver can access the required number of satellites in most cases, signal quality often degrades due to various factors, especially in urban areas with tall buildings or dense vegetation.

[0086] The proposed framework resembles a filter that takes the typical longitude and latitude output from a GPS device as input and performs a "correction operation" on the estimated longitude and latitude to provide a more accurate output. When a new data point is received, a reinforcement learning model trains an agent to determine the number of "units" needed to adjust the observed longitude and latitude to return a more accurate location. The process is as follows: Figure 2 As shown in (b), from a decision theory perspective, this sequential decision problem can be modeled as a partially observable Markov decision process (POMDP). The goal of the model is to learn a policy π(a|z,θ), where a represents the action vector, z represents the observation vector, and θ represents the model parameter vector. The policy aims to parameterize the conditional probability of performing action a given observation z to maximize its reward.

[0087] The self-localization filtering and fusion module obtains the vehicle's accurate position information by filtering and fusing information from the reinforcement learning module, IMU module, in-vehicle data acquisition module, and external data acquisition module.

[0088] The relative positioning filtering and fusion module obtains the position information of other cooperating vehicles by filtering and fusing the vehicle's own position information obtained by the self-positioning fusion filtering module with the relative position and speed estimates relative to other cooperating vehicles obtained from the external data acquisition module.

[0089] The global positioning filtering and fusion module obtains more accurate global vehicle position information by filtering and fusing the vehicle's own position information with the relative position estimation information from other cooperating vehicles.

[0090] The communication device is used to realize communication between intelligent vehicles or communication between intelligent vehicles and roadside units (RSUs) in existing intelligent transportation systems.

[0091] A multi-sensor, multi-vehicle cooperative localization method, such as Figure 3 As shown, the implementation of the multi-sensor, multi-vehicle cooperative positioning system described above includes the following steps:

[0092] Step 1: The GPS receiving module acquires, tracks, synchronizes, and frames satellites and performs positioning calculations to obtain the longitude, latitude, and elevation of the vehicle's location. The IMU module obtains estimates of the vehicle's angular velocity, velocity, and acceleration. The in-vehicle data acquisition module collects wheel speed data, steering angle, and mileage data. Simultaneously, the external data acquisition module obtains estimates of the vehicle's relative position and velocity compared to other cooperating vehicles.

[0093] Step 2: The reinforcement learning module takes the longitude and latitude outputs from the GPS receiver as input and performs correction operations on the estimated longitude and latitude to provide a more accurate location output. When a new data point is received, the reinforcement learning model trains an agent to determine the number of "units" that need to be adjusted for the observed longitude and latitude to return a more accurate location. This sequential decision problem can be modeled as a partially observable Markov decision process (POMDP). The goal of the model is to learn a policy π(a|z,θ), where a represents the action vector, z represents the observation vector, and θ represents the model parameter vector. The goal of the policy is to parameterize the conditional probability of performing action a given observation z to maximize its reward.

[0094] The following details the proposed reinforcement learning model: 1) Action Space: Actions are defined as longitude-latitude update operations. To reduce computational complexity, continuous longitude and latitude values ​​are discretized into small step sizes. Generally, discretizing each dimension of the action space is not recommended, as this exponentially increases the size of the policy table. However, in low-dimensional action spaces, discretizing the action space can help reduce the computational complexity of the algorithm, as demonstrated in this problem;

[0095] 2) Observation and Model Input: GPS devices report their locations at a certain frequency. In the proposed reinforcement learning model, an observation is not limited to the last reported GPS location, but is a stacked vector containing the history of the last reported location and the most recently predicted point. That is, instead of using the reported GPS trajectory, model predictions are used to form the observation history vector. It is important to note that the prediction frequency can be set to a value higher than the GPS data collection frequency. By forming the observation vector in this way, the model can utilize the historical trajectory information of the GPS device and the model's performance to learn a high-quality policy to correct the reported GPS points.

[0096] Let the GPS reporting point at time t be denoted as q. t Its actual position is represented as g t Since the true location is unknown, this problem can be formulated as a partially observable Markov decision process (POMDP). In this POMDP, p...t Indicates GPS reporting point q t The confidence state, which is represented as p in the RL model. t Poupart and Bouliier pointed out that in POMDP, the optimal action policy can be determined by considering a fully observable confidence-state Markov decision process (MDP), where confidence states form states and policy π maps actions to confidence states; that is, partially observable states are replaced by their estimates, i.e., confidence states, to form the MDP (POMDP is considered as an MDP). An observation buffer Z of size N is used. t To store the historical model estimates of the most recent N-1 GPS reporting points and the current q t That is, Z t ={p t-N-1 ,…,p t-1 ,q t Let's use S. t and b t Z t The hidden state and confidence state. Given an observation buffer of size N, at time t, vector S t The corresponding real location buffer containing these points, i.e., S t ={g t-N ,…,g t Vector b t The estimate includes the most recent N points, i.e., b t ={p t-N ,…,p t Please note that Z t and b t Z differs only in its last element. t The last element is q t And vector b t The last element is p t The model is based on Z. t Estimate b t .

[0097] Based on the POMDP setup described above, the goal of the reinforcement learning agent at each timett is to find the optimal correction action to correct q. t This process is based on a sliding window. Once a new q is received... t The sliding window moves forward one step, forming a new observation vector of size N, where q t The final element, the last N-1 beliefs, constitute the observation vector Z. t The rest of the text.

[0098] Whenever the GPS device reports a new location q tAt this time, the model will be trained and move to the next observation buffer. When p is obtained via GPS device t At that time, it is pushed into the observation buffer to replace q. t The observations are then moved to time t+1. That is, in each training step, the reinforcement learning model's observations include the observed GPS points and a series of historical estimates;

[0099] 3) Confidence Reward: In robotics, the confidence ellipse is used to measure the predictive performance of reinforcement learning models in Simultaneous Localization and Mapping (SLAM) problems. Assuming the noise from GPS observations is a white Gaussian distribution, the confidence ellipse reflects the uncertainty of the model's predictions. A smaller confidence ellipse indicates confidence in the model's predictions; otherwise, a larger area indicates confidence. When a new GPS point is observed at time t, the model is trained k times, not just once, based on the model parameters obtained at time t-1 and the new observation input, yielding k possible output predictions. Two metrics are used to measure the impact of uncertainty on the reinforcement learning model's performance. Both metrics are based on the covariance matrix of the surrogate's predictions. This covariance matrix is ​​a two-dimensional matrix representing the degree of certainty regarding the direction of the correction action for the longitude and latitude of the GPS observations. Let the eigenvalues ​​of this covariance matrix be a and b. If both a and b are sufficiently distinct from zero, their product is used to calculate the area of ​​the confidence ellipse, with the formula πab. Otherwise, if at least one of these values ​​is close to zero, a+b is used as the surrogate measure of prediction confidence. A larger confidence measure indicates lower uncertainty. We will later use the concept of uncertainty to construct a reward function.

[0100] While maximizing the confidence measure can minimize the predicted covariance, the predicted location may not necessarily lie within the road constraints. Therefore, this model utilizes digital map information to improve predictions, incorporating map matching into the reward function. Map matching algorithms use a matching process to map observed data points (x, y) onto a road. Therefore, they can be viewed as a simple search problem. However, map matching can be inaccurate when the road network is too complex or when the observed data points deviate significantly from their true locations on the ground (e.g., in urban areas). Therefore, this work incorporates map matching as a regularization term only in the proposed model. Assuming the map matching result for the observation set (X, Y) is... Define the reward for one step as:

[0101] r=z+γ×D

[0102] Where Z is the confidence measure, γ is the regularization parameter, and D is the sum of the negative squared errors between the observed values ​​and the map matching results; that is... (x i ,y i )∈(X,Y),

[0103] Given a strategy π, define τ = {a0, O0, r1, a1, O1, r2, ..., a T-1 O T-1 ,r T-1 Let ,} represent a trajectory of the POMDP, and return the total reward along trajectory τ as the final reward of the policy. Using a discount factor η, the reward function is expressed as:

[0104]

[0105] The goal of the model is to learn a policy that maximizes the discount reward return value R in each prediction. τ Set T=8 to pass R t To approximate R τ Perform "n-step rewards";

[0106] 4) A3C Training Architecture: The A3C training architecture requires shorter training sessions and provides more robust policies, while outperforming traditional reinforcement learning algorithms (such as DQN and one-step Q-learning) (i.e., producing higher rewards). A3C runs agents in parallel on multiple threads. It provides agents with diverse training environments by allowing each agent to retain a copy of its state / observations and train an independent model. The agent then quantifies its "advantage," a metric that measures the quality of its actions.

[0107] Step 3: The self-localization filtering and fusion module takes the outputs of the reinforcement learning module, IMU module, and in-vehicle data acquisition module as inputs, and uses a Kalman filter (KF) to perform filtering and fusion to obtain the vehicle's own position information.

[0108] Setting the state variables of the Kalman filter KF

[0109] Where, x i y i These are the coordinates of vehicle i in the Cartesian coordinate system. Let be the speed of vehicle i; then the state transition equation and measurement equation of the Kalman filter KF are as follows:

[0110] x[k]=f(x[k-1],u[k],ω[k])

[0111] z[k] = g(x[k], v[k])

[0112] Where k is a discrete-time index, x is the vehicle's state including position and velocity, u is the command process, equivalent to driving input, representing acceleration, ω is command noise or state noise, which comes from the uncertainty of the command process; z is the measurement data reported by various sensors such as IMU, GPS, Radar, and camera; v is the data noise in the measurement and transmission process; f and g are the state equation and measurement model obtained from the physical dynamics of motion and the inherent characteristics of the sensing device, respectively.

[0113] The Kalman filter (KF) for filtering and fusion includes the following two steps:

[0114] Step S1: Time Update: Calculate the prior state and state transition Jacobian matrix to evaluate the prediction covariance;

[0115] Step S2: Measurement Update: Calculate the observation Jacobian matrix and Kalman filter gain;

[0116] Step 4: The relative positioning filtering and fusion module takes the output of the self-positioning filtering and fusion module and the relative position and speed estimates relative to other cooperating vehicles obtained from signal acquisition as input, and uses a Kalman filter (KF) for filtering and fusion to obtain the position information of other cooperating vehicles.

[0117] Step 5: Through communication devices, intelligent connected vehicles communicate with each other, exchanging the location information between cooperating vehicles obtained in Step 4. Furthermore, if a Remote Unit (RSU) exists within the intelligent transportation system, the RSU will be identified during vehicle localization, and the relative position from the RSU to the vehicle will be obtained. Simultaneously, the RSU will broadcast its absolute position coordinates to vehicles within its communication range. By calculating this information, the vehicle obtains its own absolute position estimate. Since the RSU's position information is reliable and accurate, with errors originating only from the vehicle's sensing devices, incorporating the RSU into the cooperation improves localization accuracy.

[0118] Step 6: The global filtering and fusion module takes the output of the self-localization filtering and fusion module in Step 3 and the position estimates of itself from other cooperating vehicles or RSUs in Step 5 as inputs, and uses the Kalman filter (KF) to perform filtering and fusion, resulting in a more accurate position estimate of the vehicle itself.

[0119] The goal of global filtering is to calculate the vehicle V based on the output of the local filter. S Optimal position estimation for vehicle V; S Self-estimation of state It is represented, and includes the covariance matrix P. S And from vehicle V i The local estimate is then used It is represented, and includes the covariance matrix P. iFor most sensing devices, the measurement results conform to a Gaussian distribution, so the local filtering results are also Gaussian distributed. Therefore, the global optimal state estimate is expressed as a linear combination of local estimates. In other words, the problem becomes a data fusion problem under a linear Gaussian system, which is similar to data fusion of multiple sensors in a single vehicle.

[0120] Under the above assumptions, global estimation Recorded as:

[0121]

[0122] Where A i and A s Let A be the unknown weights of the linear combination to be solved. i Weights estimated for other vehicles, A s These are the self-estimated weights; The variance is:

[0123]

[0124] To ensure the unbiasedness of the global estimate, the mean of the estimate cannot be changed; therefore, A i Constraints:

[0125]

[0126] Based on the Gaussian assumption above, The maximum likelihood estimate is the estimate that minimizes the variance; therefore, global filtering becomes an optimization problem.

[0127]

[0128]

[0129] The Lagrange multiplier method is used to solve the convex optimization problem, thus obtaining the objective function:

[0130]

[0131] Finally, the optimal weights for the linear combination are obtained at the global filter:

[0132]

[0133]

[0134] The weights are inversely proportional to the local filtering performance;

[0135] Therefore, the final result of the global optimal estimate is:

[0136]

[0137] For a vehicle that senses and communicates with the RSU, considering the measurement of the RSU, the globally optimal estimate is expressed as:

[0138]

[0139] in:

[0140]

[0141]

[0142]

[0143] Because RSUs possess reliable and accurate location information, they improve the positioning accuracy of vehicles within their communication range. On the other hand, due to cooperation between vehicles, vehicles that receive assistance from RSUs further assist vehicles that cannot directly perceive or communicate with RSUs, thereby improving their positioning accuracy and thus enhancing the positioning and tracking performance of vehicles throughout the network.

Claims

1. A multi-sensor, multi-vehicle cooperative localization method, characterized in that, Includes the following steps: Step 1: The GPS receiving module acquires, tracks, synchronizes, and frames satellites and performs positioning calculations to obtain the longitude, latitude, and elevation of the vehicle's location. The IMU module obtains estimates of the vehicle's angular velocity, velocity, and acceleration. The in-vehicle data acquisition module collects wheel speed data, steering angle, and mileage data. Simultaneously, the external data acquisition module obtains estimates of the vehicle's relative position and velocity compared to other cooperating vehicles. Step 2: The reinforcement learning module takes the longitude and latitude output of the GPS receiver as input and performs correction operations on the estimated longitude and latitude to provide a more accurate location output. Step 3: The self-localization filtering and fusion module takes the outputs of the reinforcement learning module, IMU module, and in-vehicle data acquisition module as inputs, and uses a Kalman filter (KF) to perform filtering and fusion to obtain the vehicle's own position information. Step 4: The relative positioning filtering and fusion module takes the output of the self-positioning filtering and fusion module and the relative position and speed estimates relative to other cooperating vehicles obtained from signal acquisition as input, and uses a Kalman filter (KF) for filtering and fusion to obtain the position information of other cooperating vehicles. Step 5: Through the communication device, the intelligent connected vehicles communicate with each other and exchange the location information between the cooperating vehicles obtained in Step 4. Furthermore, if the intelligent transportation system in which the vehicle is located has a Remote Unit (RSU), the RSU will be identified during the vehicle localization process, and the relative position from the RSU to the vehicle will be obtained. At the same time, the RSU will broadcast its own absolute position coordinates to vehicles within its communication range. By calculating this information, the vehicle obtains its own absolute position estimate. Since the RSU's position information is reliable and accurate, the error only comes from the vehicle's sensing devices. Therefore, incorporating RSUs into the collaboration improves the accuracy of positioning; Step 6: The global filtering and fusion module takes the output of the self-localization filtering and fusion module in Step 3 and the position estimates of itself from other cooperating vehicles or RSUs in Step 5 as inputs, and uses the Kalman filter (KF) to perform filtering and fusion, resulting in a more accurate position estimate of the vehicle itself. The reinforcement learning module mentioned in step 2 includes: 1) Action Space: The action is defined as a longitude-latitude update operation; in order to reduce computational complexity, continuous longitude and latitude values ​​are discretized into small step sizes; 2) Observation and Model Input: GPS devices report their locations at a certain frequency; in the proposed reinforcement learning model, an observation is not limited to the last reported GPS location, but is a stacked vector containing the last reported location and the history of the most recently predicted point; instead of using the reported GPS trajectory, model predictions are used to form the observation history vector; the prediction frequency is set to a value higher than the GPS data collection frequency; by forming the observation vector in this way, the model utilizes the historical trajectory information of the GPS device and the performance of the model to learn a high-quality policy to correct the reported GPS points; Time GPS reporting points are represented as Its actual location is represented as Since the true location is unknown, the problem is formulated as a partially observable Markov decision process (POMDP); in this POMDP, the following is used: Indicates GPS reporting point The confidence state, which is represented in the RL model as Some observable states are replaced by their estimates, i.e., confidence states, to form an MDP; using a size of observation buffer To store the most recent Historical model estimates and current GPS reporting points ;Right now ;use and They represent Hidden states and confidence states; given a size of The observation buffer, in time ,vector The corresponding real location buffer containing these points, i.e. ;vector Including recent An estimate of a point, i.e. ; and It differs only in its last element. The last element is And vector The last element is The model is based on estimate ; Based on the POMDP setup described above, the goal of the reinforcement learning agent at each timett is to find the optimal correction action to rectify the error. This process is based on a sliding window; once a new [window] is received... The sliding window moves forward one step, forming a new observation vector with a size of ,in The last element, the final Each belief constitutes an observation vector. The rest; Whenever the GPS device reports a new location At that time, the model will be trained and move to the next observation buffer; when data is acquired via GPS device... At that time, it is pushed into the observation buffer to replace The observation then shifts to time. In each training step, the reinforcement learning model's observations include observed GPS points and a series of historical estimates. The multi-sensor multi-vehicle cooperative positioning method is based on a multi-sensor multi-vehicle cooperative positioning system, which includes a signal acquisition device, a central processing unit, and a communication device. The signal acquisition device is responsible for acquiring the location information of the vehicles, the communication device is responsible for the interaction of signals acquired by each vehicle, and the central processing unit is responsible for fusing and filtering the signals from its own signal acquisition device and communication device to obtain the accurate location information of the vehicle itself.

2. The multi-sensor, multi-vehicle cooperative positioning method according to claim 1, characterized in that, The signal acquisition device includes a GPS receiving module, an IMU module, an in-vehicle data acquisition module, and an external data acquisition module; the central processing unit includes a reinforcement learning module, a self-localization filtering and fusion module, a relative positioning filtering and fusion module, and a global positioning filtering and fusion module.

3. The multi-sensor, multi-vehicle cooperative positioning method according to claim 2, characterized in that, The GPS receiving module is used to receive satellite signals, send the received signals to the reinforcement learning module, and finally send them to the self-localization filtering and fusion module. The IMU module is used to obtain the vehicle's angular velocity, velocity, and acceleration estimates, and sends the measured data to the self-localization filtering and fusion module; The in-vehicle data acquisition module is used to collect the vehicle's wheel speed data, steering angle and mileage data, and input these data as input variables into the self-localization filtering fusion module; The external data acquisition module includes a camera and radar, used to obtain relative position and speed estimates relative to other cooperating vehicles; The reinforcement learning module is used to improve the positioning accuracy of GPS so as to more accurately locate the vehicle in the case of autonomous or semi-autonomous driving; the goal is to find the optimal correction strategy for the observed GPS longitude and latitude coordinates to generate more accurate location information. The self-localization filtering and fusion module obtains the vehicle's accurate position information by filtering and fusing information from the reinforcement learning module, IMU module, in-vehicle data acquisition module, and external data acquisition module. The relative positioning filtering and fusion module obtains the position information of other cooperating vehicles by filtering and fusing the vehicle's own position information obtained by the self-positioning fusion filtering module with the relative position and speed estimates relative to other cooperating vehicles obtained from the external data acquisition module. The global positioning filtering and fusion module obtains more accurate global vehicle position information by filtering and fusing the vehicle's own position information with the relative position estimation information from other cooperating vehicles. The communication device is used to enable communication between intelligent vehicles or between an intelligent vehicle and a roadside unit (RSU) in an existing intelligent transportation system.

4. The multi-sensor, multi-vehicle cooperative positioning method according to claim 1, characterized in that, Step 2 is as follows: When a new data point is received, the reinforcement learning model trains an agent to determine the number of "units" that the observed longitude and latitude need to be adjusted in order to return a more accurate location. This sequential decision problem is modeled as a partially observable Markov decision process (POMDP); the goal of the model is to learn a policy. ,in Represents the action vector. Represents the observation vector. This represents the model parameter vector; the goal of the policy is to parameterize the model given a certain observation. Execute action at time The conditional probability is used to maximize one's reward.

5. The multi-sensor, multi-vehicle cooperative positioning method according to claim 1, characterized in that, The reinforcement learning model described in step 2 also includes: 3) Confidence Reward: The noise from GPS observations follows a white Gaussian noise distribution. The confidence ellipse reflects the uncertainty of the model's predictions. A smaller confidence ellipse indicates confidence in the model's predictions; conversely, a larger area indicates confidence. When a new GPS point is observed at time t, the model is trained k times, not just once, based on the model parameters obtained at time t-1 and the new observation input, yielding k possible output predictions. Two metrics are used to measure the impact of uncertainty on the reinforcement learning model's performance. Both metrics are based on the covariance matrix of the surrogate's predictions. This covariance matrix is ​​a two-dimensional matrix representing the degree of certainty regarding the direction of the correction action for the longitude and latitude of the GPS observations. Let the eigenvalues ​​of this covariance matrix be... and ;if and Both are sufficiently different from zero, and their product is used to calculate the area of ​​the confidence ellipse, using the formula: Otherwise, if at least one of the values ​​is close to zero, use As a proxy measure of prediction confidence; the higher the confidence measure, the lower the uncertainty; the reward function is constructed using the concept of uncertainty; Although maximizing the confidence measure minimizes the predicted covariance, the predicted location may not necessarily be within the road constraints. Therefore, this model utilizes digital map information to improve predictions, incorporating map matching into the reward function. The map matching algorithm uses a matching process to combine observed data points... Mapped onto a road; therefore, they are treated as a simple search problem; map matching may be inaccurate when the road network is too complex or the observed data points deviate too much from the true ground location; therefore, map matching is included as a regularization term only in the proposed model; observation set The map matching result is Define the reward for one step as: Where Z is the confidence level measure. It is a regularization parameter. It is the sum of the negative squared errors between the observed values ​​and the map matching results; that is... , , ; Given a strategy Below, definition For a trajectory of POMDP, the trajectory The total reward is returned as the final reward of the strategy; a discount factor is used. The reward function is expressed as: The goal of the model is to learn a policy to maximize the discount reward return value in each prediction. ;set up To pass To approximate Perform "n-step rewards"; 4) A3C Training Architecture: The A3C training architecture used requires shorter training sessions and provides more robust policies. A3C runs agents in parallel on multiple threads; it provides a diverse training environment for agents by allowing each agent to retain a copy of its state or observations and train an independent model.

6. The multi-sensor, multi-vehicle cooperative positioning method according to claim 1, characterized in that, Step 3 specifically involves: Setting the state variables of the Kalman filter KF =[ , , , ] in, It is a vehicle Coordinates in the Cartesian coordinate system It is a vehicle The speed; then the state transition equation and measurement equation of the Kalman filter KF are as follows: in, For discrete time points, The vehicle's state includes its position and speed. This is a command process, equivalent to driving input, representing acceleration. This refers to command noise or state noise, arising from uncertainties in the command process. Measurement data reported by various sensors including IMU, GPS, Radar, and cameras. To reduce data noise during measurement and transmission; and These are the state equations and measurement models derived from the physical dynamics of motion and the inherent characteristics of the sensing device, respectively. The Kalman filter (KF) for filtering and fusion includes the following two steps: Step S1: Time Update: Calculate the prior state and state transition Jacobian matrix to evaluate the prediction covariance; Step S2: Measurement Update: Calculate the observation Jacobian matrix and Kalman filter gain.

7. The multi-sensor, multi-vehicle cooperative positioning method according to claim 1, characterized in that, Step 6 specifically involves: The goal of global filtering is to calculate the vehicle based on the output of the local filter. Optimal position estimation for vehicles; Self-estimation of state Represented, and accompanied by a covariance matrix And from vehicles The local estimate is then used Represented, and accompanied by a covariance matrix The local filtering results are also Gaussian distributed; therefore, the global optimal state estimate is expressed as a linear combination of the local estimates. Global estimation Recorded as: in and The unknown weights are the linear combinations to be solved. Weights estimated for other vehicles, These are the self-estimated weights; The variance is: To ensure the unbiasedness of the global estimate, the mean of the estimate cannot be changed; therefore... Constraints: The maximum likelihood estimate is the estimate that minimizes the variance; therefore, global filtering becomes an optimization problem. The Lagrange multiplier method is used to solve the convex optimization problem, thus obtaining the objective function: Finally, the optimal weights for the linear combination are obtained at the global filter: The weights are inversely proportional to the local filtering performance; Therefore, the final result of the global optimal estimate is: For a vehicle that senses and communicates with the RSU, considering the measurement of the RSU, the globally optimal estimate is expressed as: in: Because RSUs possess reliable and accurate location information, they improve the positioning accuracy of vehicles within their communication range. On the other hand, due to cooperation between vehicles, vehicles that receive assistance from RSUs further assist vehicles that cannot directly perceive or communicate with RSUs, thereby improving their positioning accuracy and thus enhancing the positioning and tracking performance of vehicles throughout the network.

Images