A vehicle-road multi-sensor fusion collaborative positioning method based on LoFTR and Yolo algorithm

Abstract

The present invention provides a vehicle-road multi-sensor fusion collaborative positioning method based on LoFTR and Yolo algorithms. Specifically, it first uses a data fusion scheme with extended Kalman filtering as the core to loosely couple visual navigation information and inertial navigation information to solve the problem of vehicle-side navigation data fusion, and enables the positioning system to adapt to scenes with large turning angles. Then, a coordinate system definition scheme for the vehicle side and the roadside side is proposed, so that the roadside side is responsible for eliminating the accumulated errors on the vehicle side, and a vehicle detection method is proposed on the roadside side to achieve real-time tracking of the vehicle and recording of positioning data. This scheme provides a new solution for positioning scenarios where GPS signals are obscured. It has a relatively low cost and can meet the positioning accuracy requirements. It can be used not only in outdoor road scenes, but also in indoor underground parking lots or other mobile device scenes.

Description

Technical Field

[0001] The invention relates to a vehicle-road multi-sensor fusion collaborative positioning method based on LoFTR and Yolo algorithms, and belongs to the technical field of vehicle-road collaborative positioning. Background Art

[0002] In the existing vehicle-road and multi-sensor fusion collaborative positioning systems, there are often problems with environmental complexity, positioning accuracy and robustness. Traditional positioning systems often rely on a single sensor, such as GNSS or inertial measurement unit (IMU), which may not provide accurate and reliable positioning information in some cases. For example, high-rise buildings in urban environments, indoor underground parking lots, signal shielding or sensor errors may lead to reduced positioning accuracy.

[0003] There are still some defects and challenges in vehicle-road collaborative positioning technology, which are mainly manifested in the following aspects: the primary problem is how to effectively fuse data with different accuracy and noise characteristics generated by different sensors and maintain accuracy, and for visual positioning methods, it is often impossible to match due to excessive steering angles; at the same time, when a vehicle is driving normally on the road, it is often difficult to encounter places that have been explored, which also makes it difficult to achieve loop closure. Summary of the invention

[0004] Purpose of the invention: In view of the deficiencies in the prior art, the present invention designs a vehicle-road multi-sensor fusion collaborative positioning system and method based on LoFTR+Yolo to solve the above problems. As for the problem of how to fuse different sensor data, this scheme proposes a data fusion scheme with extended Kalman filtering as the core to loosely couple visual navigation information and inertial navigation information. This scheme can also effectively solve the problem of excessive steering angle. As for the problem of loop closure difficulties, this scheme proposes a coordinate system definition scheme for the vehicle side and the roadside side. The roadside side is responsible for eliminating the accumulated errors on the vehicle side, and a vehicle detection and tracking method is proposed on the roadside side.

[0005] Technical solution: A vehicle-road multi-sensor fusion collaborative positioning method based on LoFTR and Yolo algorithm, including the following steps:

[0006] S1. Sensor deployment and navigation data acquisition: including using monocular cameras and binocular cameras on the vehicle side and the roadside side to provide visual navigation information, and using an inertial measurement unit (IMU) on the vehicle side to provide inertial navigation information;

[0007] S2. Establishment of positioning coordinate system: The positioning system includes the vehicle-side coordinate system and the roadside coordinate system, and its function is to unify the two to realize the vehicle driving trajectory drawing;

[0008] S3. Vehicle-road navigation information processing and fusion: The vehicle's own navigation information is integrated with the navigation information formed by the roadside's detection of the vehicle, forming navigation information with smaller positioning error than the vehicle or roadside positioning alone, and completing the elimination of the accumulated positioning error of the vehicle.

[0009] The S1 is specifically:

[0010] S1.1: A monocular camera is used on the vehicle side to provide visual navigation information during driving. During driving, the on-board monocular camera captures images in front of the vehicle at fixed intervals, and then the images are pre-processed according to the built-in parameters of the camera.

[0011] S1.2: Use the inertial measurement unit (IMU) on the vehicle side to obtain inertial navigation information. During the vehicle's driving, first complete the initial state balance to obtain the gyroscope angular velocity and vehicle acceleration;

[0012] S1.3: Deploy a binocular camera at the road end to detect the target information on the vehicle end, and deploy a laser rangefinder to measure the distance information between the vehicle end and the left and right targets of the binocular camera.

[0013] The S2 is specifically:

[0014] S2.1, Vehicle-side coordinate system: It is updated in real time during the vehicle's driving process, with the vehicle's current position as the coordinate origin and the current driving direction as the positive half axis of the coordinate system's X-axis;

[0015] S2.2, Roadside coordinate system: The roadside binocular camera is fixed at a position where the shooting direction is greater than 0° and less than 90° in front of the vehicle's driving direction, with the left eye position of the roadside binocular camera as the coordinate origin, and the direction from the left eye to the right eye of the roadside binocular camera as the positive half axis of the X-axis.

[0016] The S2.2 is specifically:

[0017] The formula for calculating the polar coordinates of the vehicle end in the road end coordinate system is as follows:

[0018]

[0019] Where: When the roadside detects the vehicle, the roadside laser rangefinder is used to obtain the distance between the vehicle and the left camera on the roadside as D A1 , the distance from the right camera is D B1 , the distance between the left and right cameras is D AB The angle between the line formed by the vehicle end and the roadside end and the positive half axis of the X axis of the roadside coordinate system is θ 1AB ,

[0020] The S3 is specifically:

[0021] S3.1. Vehicle-side visual navigation information processing: The vehicle-side uses the LoFTR method as a visual image processing method. The LoFTR method uses Transformers as the basic model, including feature extraction module, local feature module, coarse matching module and coarse-grained to fine-grained module;

[0022] The feature extraction module contains a local feature CNN (convolutional neural network) with FPN (feature pyramid network); the local feature module contains two layers of Transformers, a self-attention layer and a cross-attention layer; the coarse matching module contains the screening of matching confidence, and the softmax activation function is used for classification; the coarse-grained to fine-grained module contains matching refined to the resolution level;

[0023] S3.2. Vehicle-side inertial navigation information processing: The vehicle side is equipped with an inertial measurement unit (IMU) as a sensor to measure the linear acceleration and angular velocity of the vehicle;

[0024] The parameters output by the IMU include the linear acceleration vector a and the angular velocity vector ω, which are used to obtain the position change of the vehicle;

[0025] S3.3, Fusion of vehicle-side vision and inertial navigation information: During the vehicle's operation time, if the steering angle of the vehicle between two consecutive shooting moments of the monocular camera exceeds 45 degrees, resulting in the visual overlap area between the two moments being less than 50%, IMU data is used instead of visual data;

[0026] When the steering angle of the vehicle between two consecutive shots taken by the monocular camera is less than 45 degrees, the extended Kalman filter is used to fuse the information of visual navigation and inertial navigation to estimate the vehicle state.

[0027] S3.4, roadside visual navigation information processing: the roadside uses a deep learning-based target detection method to detect and track the vehicle captured by the roadside binocular camera. The deep learning-based target detection method has a basic structure that includes three modules: a backbone network, a feature pyramid network, and a prediction head. The backbone network uses CSPDarknet as the basic model, including a lightweight Darknet convolutional network. The convolutional network uses 20 convolutional layers and 4 pooling layers, and uses residual connections to transfer inter-layer outputs. At the same time, the backbone network also has a CSP structure that realizes forward and cross-stage connection functions;

[0028] The feature pyramid module includes convolutional layers and upsampling operations to process feature maps of different scales; the prediction head module includes convolutional layers and activation functions to generate bounding boxes and category probabilities of targets at different scales;

[0029] S3.5, vehicle-road navigation information fusion: When the roadside binocular camera detects the vehicle, the distance between the left and right binoculars and the vehicle is obtained, and the polar coordinates of the vehicle are obtained according to S2.2 above (D A1 ,θ 1AB ), the exact position of the roadside end in the world coordinate system is known, and the precise coordinates (x, y) of the vehicle end in the world coordinate system are obtained by conversion. The vehicle end uses this coordinate to initialize its own positioning to eliminate the accumulated error.

[0030] The S3.1 is specifically:

[0031] The input of the LoFTR model is an image pair I A ,I B , after the feature extraction module, the coarse-grained feature image pair F of 1 / 8 of the original image is extracted A 、F B The fine-grained feature image pair E of 1 / 2 of the original image A 、E B ; The coarse-grained feature image pair passes through the local feature module, is flattened into a 1-D vector, and then adds the position encoding, and then passes through N iterations of the self-attention layer and the cross-attention layer; the coarse-grained feature image pair after iteration passes through the coarse matching module to obtain the confidence matrix P c , and generate a rough matching prediction M c , according to the mutual nearest neighbor criterion (MNN), calculate M c The formula is as follows:

[0032]

[0033] Where: The confidence threshold is θ c , the pixel is represented as (i,j), P c (i,j) represents the matching confidence at pixel (i,j).

[0034] The rough matching prediction results go through the coarse-grained to fine-grained module. c , a local window of size w×w is cut from the fine-grained feature image pair, and the matching result is refined to the pixel level, and finally the fine matching result M is obtained. f Output;

[0035] For the feature point pairs in the two images before and after the LoFTR method, the RANSAC (random sample consistency method) method is used for screening. The specific steps of the screening are:

[0036] First, use three feature point pairs to calculate the fitted plane equation, then bring all feature point pairs into the fitted plane equation model, calculate the point pairs whose errors are within the threshold, called inliers, count the number of inliers, and then iterate the process, constantly compare the number of inliers, and retain the fitting model with the largest number of inliers. The calculation formula for the number of iterations is as follows:

[0037]

[0038] In formula (3), k is the number of iterations of the RANSAC method, t is the proportion of inliers in the data, N is the number of feature point pairs, and P is the probability of collecting the correct inliers to calculate the fitting model;

[0039] The processed feature point pairs are used to realize 3D-2D pose estimation using the PnP problem solving method. Based on the pose estimation, the coordinate changes of the vehicle in the front and back images are calculated to update the vehicle-side coordinate system and draw the vehicle's driving trajectory.

[0040] The S3.2 is specifically:

[0041] First, the discrete time series output by the IMU is integrated to obtain the velocity v and position p of the vehicle in the world coordinate system. The calculation formula is as follows:

[0042]

[0043] In formula (4), v0 and p0 represent the initial velocity and initial position respectively;

[0044] Integrate the angular velocity to obtain the vehicle's direction angle θ, which is calculated as follows:

[0045] θ(t)=∫ω(t)dt+θ0#(5)

[0046] In formula (5), θ0 represents the initial direction angle;

[0047] Finally, by fusing the position and orientation of the vehicle, the absolute position and orientation of the vehicle in the world coordinate system is obtained. wb , where T wb is a homogeneous transformation matrix, which represents the transformation from the vehicle coordinate system to the world coordinate system; R(t) is a rotation matrix, and the calculation formula for the absolute position calculated by the vehicle's direction angle is as follows:

[0048]

[0049] By obtaining the absolute position and posture in continuous time, the coordinate changes of the vehicle in the previous and next moments are calculated, the vehicle-side coordinate system is updated, and the vehicle's driving trajectory is drawn.

[0050] The S3.3 is specifically:

[0051] The state vector x calculated for the visual navigation information vision and the state vector x calculated from the inertial navigation information imu , these two state vectors contain the vehicle's position, speed, and direction in the world coordinate system respectively; the system's state vector X is set to be the concatenation of the two system state vectors:

[0052]

[0053] Then the state equation of the system is expressed as:

[0054] x(k+1)=f(x(k),u(k))+w(k)#(8)

[0055] In formula (8), f(x(k),u(k)) represents the state transfer function, u(k) represents the external input, and w(k) represents the process noise; the measurement equation is expressed as:

[0056] z(k)=h(x(k))+v(k)#(9)

[0057] In formula (9), h(x(k)) represents the measurement function, and v(k) represents the measurement noise;

[0058] After the system state equation and measurement equation are calculated, for the extended Kalman filter, it is necessary to calculate the state prediction, covariance prediction, Kalman gain, state update and covariance update. The specific formula is as follows:

[0059]

[0060] In formula (11), F(k) is the state transfer matrix, Q(k) is the covariance of process noise, and in formula (12), H(k+1) is the measurement matrix, and R(k+1) is the covariance of measurement noise;

[0061] Through the above five steps, the function of integrating visual and inertial navigation information is realized to obtain a more accurate vehicle state estimation;

[0062] At the same time, for the problem of large vehicle steering angle, a threshold θ is defined max , represents the maximum threshold of the vehicle steering angle. At each time step k, check whether the vehicle's steering angle θ(k) is greater than θ max ;

[0063] If θ(k) is greater than the threshold, use Represents a state transfer function based only on IMU data, directly using the IMU state update:

[0064]

[0065] Otherwise, continue to use the aforementioned extended Kalman filter for state fusion.

[0066] The S3.4 is specifically:

[0067] The model input is the original image I, and the normalized input image I is obtained after preprocessing. norm ;

[0068] The preprocessing operations include scaling, normalization, and dedistortion operations to meet the input requirements of the LoFTR model network. The backbone network uses CSPDarknet and CNN (convolutional neural network) structures to transform the input image I norm Convert to a high-dimensional feature map F backbone In the feature pyramid network processing stage, the feature map F extracted by the backbone network is backbone Input into FPN (Feature Pyramid Network), use N to represent the number of layers of the feature pyramid network, and generate a multi-scale feature map F FPN The formula is:

[0069] F FPN ={F level1 ,F level2 ,…,F levelN}#(16)

[0070] At each scale F level The prediction head transforms the feature map F level Input into the convolutional layer to generate the bounding box B of the target level and class probability C level , the calculation formula is:

[0071] B level =Sigmoid(Y box )+Grid#(17)

[0072] C level =Softmax(Y conf )#(18)

[0073] In formula (17), Y box is the bounding box coordinate offset of the network output, Sigmoid(Y box ) is the Sigmoid function, Grid is the grid of the anchor box, and in formula (18), Y conf is the category probability score output by the network; finally, the overlapping bounding boxes are filtered out through the non-maximum suppression (NMS) operation, and the targets with higher confidence are retained, and finally the detection results are obtained, including the location and confidence information of the vehicle end;

[0074] For the vehicle end that is successfully detected and tracked, the roadside end uses the calculation method of the roadside coordinate system in S2.2 above to obtain and record the polar coordinates of the vehicle end in the roadside coordinate system, and then generates the vehicle running trajectory based on this.

[0075] The S3.5 is specifically:

[0076] After the vehicle is initialized, it restarts its own trajectory calculation. In the process of leaving the roadside detection, the calculated trajectory is filtered and fused with the vehicle-side running trajectory obtained by the roadside using the polar coordinate calculation method. The particle filter is used as an example to illustrate the vehicle-road navigation information fusion process:

[0077] First, a group of particles are selected from the trajectories calculated by the vehicle-mounted end and the roadside end respectively to represent the possible position of the vehicle in the coordinate system. Then, the steps of updating, resampling, particle fusion, state estimation and state updating are performed according to the following formula, and the final vehicle-road fusion positioning result is obtained after n iterations:

[0078]

[0079] Formula (19) is the motion model update of the vehicle side, represents the position of the i-th vehicle-end particle at time step k+1, represents the position of the i-th vehicle-end particle at time step k, Indicates speed, represents acceleration, Δt represents the time step, represents the system process noise at time step k; formula (20) is the update of the roadside motion model; formula (21) is the measurement update of the vehicle side, represents the weight of the i-th vehicle-end particle at time step k+1, Represents the state of a given particle Under these conditions, the measured value z k+1,vehicle The probability density function of ; Formula (22) is the roadside measurement update; Formula (23) is the calculation of the normalized weight; Formula (24) is the state estimation, represents the state estimate at time step k+1, represents the position of the ith particle at time step k+1; formula (25) is the state update, z k+1 is the most recent measurement.

[0080] Beneficial effects: Aiming at the problem of how to fuse data with different precision and noise characteristics measured by different sensors, the present invention proposes a method for fusing visual and inertial navigation information, which effectively improves the accuracy of positioning and solves the problem that pure visual positioning is difficult to cope with scenes with large steering angles. Aiming at the problem of accurate matching of vehicle-side and road-side sensor data, the present invention proposes a multi-source fusion location re-identification matching method that can accurately identify nodes of vehicle-road collaboration.

[0081] The present invention aims to solve the problem that vehicles often fail to encounter previously explored locations, i.e., it is difficult to loop back. The present invention proposes a vehicle-road fusion positioning method, which enables the vehicle to achieve accurate initialization, eliminates cumulative errors, and increases positioning accuracy. In order to effectively fuse the vehicle-mounted sensor data and the road-side sensor data, the present invention proposes a method of establishing an observation equation using an extended Kalman filter, which can effectively fuse the vehicle-side and road-side positioning results, thereby optimizing the positioning accuracy.

[0082] In view of the problem that multi-source navigation information is difficult to fuse, the present invention proposes to realize data fusion by a filtering scheme, and realizes particle filtering fusion of vehicle-road navigation information, which can effectively fuse multi-source information and optimize positioning accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0083] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on the provided drawings without paying creative work.

[0084] Figure 1 It is the overall flow chart of the present invention.

[0085] Figure 2 Schematic diagram of the road end coordinate system of the present invention.

[0086] Figure 3 This is a flowchart of the vehicle-mounted visual feature point pair extraction of the present invention.

[0087] Figure 4 This is a schematic diagram of the deployment of the vehicle-road cooperative equipment of the present invention. DETAILED DESCRIPTION

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

[0089] In the description of the present invention, it is necessary to understand that the terms "up", "down", "front", "back", "left", "right", "vertical", "horizontal", "top", "bottom", "inside", "outside", etc., indicating the orientation or position relationship are based on the orientation or position relationship shown in the drawings, and are only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore cannot be understood as a limitation on the present invention.

[0090] In the present invention, unless otherwise clearly specified and limited, a first feature being "above" or "below" a second feature may include that the first and second features are in direct contact, or may include that the first and second features are not in direct contact but are in contact through another feature between them. Moreover, a first feature being "above", "above" and "above" a second feature includes that the first feature is directly above and obliquely above the second feature, or simply indicates that the first feature is higher in level than the second feature. A first feature being "below", "below" and "below" a second feature includes that the first feature is directly below and obliquely below the second feature, or simply indicates that the first feature is lower in level than the second feature.

[0091] like Figures 1 to 4 As shown, a vehicle-road multi-sensor fusion collaborative positioning method based on LoFTR and Yolo algorithm includes the following steps:

[0092] S1. Sensor deployment and navigation data acquisition: including using monocular cameras and binocular cameras on the vehicle side and the roadside side to provide visual navigation information, and using an inertial measurement unit (IMU) on the vehicle side to provide inertial navigation information;

[0093] S1.1: A monocular camera is used on the vehicle side to provide visual navigation information during driving. During driving, the on-board monocular camera captures images in front of the vehicle at fixed intervals, and then the images are pre-processed according to the built-in parameters of the camera.

[0094] S1.2: Use the inertial measurement unit (IMU) on the vehicle side to obtain inertial navigation information. During the vehicle's driving, first complete the initial state balance to obtain the gyroscope angular velocity and vehicle acceleration;

[0095] S1.3: Deploy a binocular camera at the road end to detect the target information on the vehicle end, and deploy a laser rangefinder to measure the distance information between the vehicle end and the left and right targets of the binocular camera.

[0096] S2. Establishment of positioning coordinate system: The positioning system includes a vehicle-side coordinate system and a roadside coordinate system, and its function is to unify the two to realize vehicle driving trajectory drawing;

[0097] S2.1, Vehicle-side coordinate system: It is updated in real time during the vehicle's driving process, with the vehicle's current position as the coordinate origin and the current driving direction as the positive half axis of the coordinate system's X-axis;

[0098] S2.2, Roadside coordinate system: The roadside binocular camera is fixed at a position where the shooting direction is greater than 0° and less than 90° in front of the vehicle's driving direction, with the left eye position of the roadside binocular camera as the coordinate origin, and the direction from the left eye to the right eye of the roadside binocular camera as the positive half axis of the X-axis.

[0099] The S2.2 is specifically:

[0100] The formula for calculating the polar coordinates of the vehicle end in the road end coordinate system is as follows:

[0101]

[0102] Where: When the roadside detects the vehicle, the roadside laser rangefinder is used to obtain the distance between the vehicle and the left camera on the roadside as D A1 , the distance from the right camera is D B1 , the distance between the left and right cameras is D AB The angle between the line formed by the vehicle end and the roadside end and the positive half axis of the X axis of the roadside coordinate system is θ 1AB ,

[0103] S3. Vehicle-road navigation information processing and fusion: The vehicle's own navigation information is integrated with the navigation information formed by the roadside's detection of the vehicle, forming navigation information with smaller positioning error than the vehicle or roadside positioning alone, and completing the elimination of the accumulated positioning error of the vehicle.

[0104] S3.1. Vehicle-side visual navigation information processing: The vehicle-side uses the LoFTR method as a visual image processing method. The LoFTR method uses Transformers as the basic model, including feature extraction module, local feature module, coarse matching module and coarse-grained to fine-grained module;

[0105] The feature extraction module contains a local feature CNN (convolutional neural network) with FPN (feature pyramid network); the local feature module contains two layers of Transformers, a self-attention layer and a cross-attention layer; the coarse matching module contains the screening of matching confidence, and the softmax activation function is used for classification; the coarse-grained to fine-grained module contains matching refined to the resolution level;

[0106] The S3.1 is specifically:

[0107] The input of the LoFTR model is an image pair I A ,I B , after the feature extraction module, the coarse-grained feature image pair F of 1 / 8 of the original image is extractedA 、F B The fine-grained feature image pair E of 1 / 2 of the original image A 、E B ; The coarse-grained feature image pair passes through the local feature module, is flattened into a 1-D vector, and then adds the position encoding, and then passes through N iterations of the self-attention layer and the cross-attention layer; the coarse-grained feature image pair after iteration passes through the coarse matching module to obtain the confidence matrix P c , and generate a rough matching prediction M c , according to the mutual nearest neighbor criterion (MNN), calculate M c The formula is as follows:

[0108]

[0109] Where: The confidence threshold is θ c , the pixel is represented as (i,j), P c (i,j) represents the matching confidence at pixel (i,j).

[0110] The rough matching prediction results go through the coarse-grained to fine-grained module. c , a local window of size w×w is cut from the fine-grained feature image pair, and the matching result is refined to the pixel level, and finally the fine matching result M is obtained. f Output;

[0111] For the feature point pairs in the two images before and after the LoFTR method, the RANSAC (random sample consistency method) method is used for screening. The specific steps of the screening are:

[0112] First, use three feature point pairs to calculate the fitted plane equation, then bring all feature point pairs into the fitted plane equation model, calculate the point pairs whose errors are within the threshold, called inliers, count the number of inliers, and then iterate the process, constantly compare the number of inliers, and retain the fitting model with the largest number of inliers. The calculation formula for the number of iterations is as follows:

[0113]

[0114] In formula (3), k is the number of iterations of the RANSAC method, t is the proportion of inliers in the data, N is the number of feature point pairs, and P is the probability of collecting the correct inliers to calculate the fitting model;

[0115] The processed feature point pairs are used to realize 3D-2D pose estimation using the PnP problem solving method. Based on the pose estimation, the coordinate changes of the vehicle in the front and back images are calculated to update the vehicle-side coordinate system and draw the vehicle's driving trajectory.

[0116] S3.2. Vehicle-side inertial navigation information processing: The vehicle side is equipped with an inertial measurement unit (IMU) as a sensor to measure the linear acceleration and angular velocity of the vehicle;

[0117] The parameters output by the IMU include the linear acceleration vector a and the angular velocity vector ω, which are used to obtain the position change of the vehicle;

[0118] The S3.2 is specifically:

[0119] First, the discrete time series output by the IMU is integrated to obtain the velocity v and position p of the vehicle in the world coordinate system. The calculation formula is as follows:

[0120]

[0121] In formula (4), v0 and p0 represent the initial velocity and initial position respectively;

[0122] Integrate the angular velocity to obtain the vehicle's direction angle θ, which is calculated as follows:

[0123] θ(t)=∫ω(t)dt+θ0#(5)

[0124] In formula (5), θ0 represents the initial direction angle;

[0125] Finally, by fusing the position and orientation of the vehicle, the absolute position and orientation of the vehicle in the world coordinate system is obtained. wb , where T wb is a homogeneous transformation matrix, which represents the transformation from the vehicle coordinate system to the world coordinate system; R(t) is a rotation matrix, and the calculation formula for the absolute position calculated by the vehicle's direction angle is as follows:

[0126]

[0127] By obtaining the absolute position and posture in continuous time, the coordinate changes of the vehicle in the previous and next moments are calculated, the vehicle-side coordinate system is updated, and the vehicle's driving trajectory is drawn.

[0128] S3.3, Fusion of vehicle-side vision and inertial navigation information: During the vehicle's operation time, if the steering angle of the vehicle between two consecutive shooting moments of the monocular camera exceeds 45 degrees, resulting in the visual overlap area between the two moments being less than 50%, IMU data is used instead of visual data;

[0129] When the steering angle of the vehicle between two consecutive shots taken by the monocular camera is less than 45 degrees, the extended Kalman filter is used to fuse the information of visual navigation and inertial navigation to estimate the vehicle state.

[0130] The S3.3 is specifically:

[0131] The state vector x calculated for the visual navigation information vision and the state vector x calculated from the inertial navigation information imu , these two state vectors contain the vehicle's position, speed, and direction in the world coordinate system respectively; the system's state vector X is set to be the concatenation of the two system state vectors:

[0132]

[0133] Then the state equation of the system is expressed as:

[0134] x(k+1)=f(x(k),u(k))+w(k)#(8)

[0135] In formula (8), f(x(k),u(k)) represents the state transfer function, u(k) represents the external input, and w(k) represents the process noise; the measurement equation is expressed as:

[0136] z(k)=h(x(k))+v(k)#(9)

[0137] In formula (9), h(x(k)) represents the measurement function, and v(k) represents the measurement noise;

[0138] After the system state equation and measurement equation are calculated, for the extended Kalman filter, it is necessary to calculate the state prediction, covariance prediction, Kalman gain, state update and covariance update. The specific formula is as follows:

[0139]

[0140] In formula (11), F(k) is the state transfer matrix, Q(k) is the covariance of process noise, and in formula (12), H(k+1) is the measurement matrix, and R(k+1) is the covariance of measurement noise;

[0141] Through the above five steps, the function of integrating visual and inertial navigation information is realized to obtain a more accurate vehicle state estimation;

[0142] At the same time, for the problem of large vehicle steering angle, a threshold θ is defined max =45°, indicating the maximum threshold of the vehicle's steering angle. At each time step k, check whether the vehicle's steering angle θ(k) is greater than θ max ;

[0143] If θ(k) is greater than the threshold, use Represents a state transfer function based only on IMU data, directly using the IMU state update:

[0144]

[0145] Otherwise, continue to use the aforementioned extended Kalman filter for state fusion.

[0146] S3.4, roadside visual navigation information processing: the roadside uses a deep learning-based target detection method to detect and track the vehicle captured by the roadside binocular camera. The deep learning-based target detection method has a basic structure that includes three modules: a backbone network, a feature pyramid network, and a prediction head. The backbone network uses CSPDarknet as the basic model, including a lightweight Darknet convolutional network. The convolutional network uses 20 convolutional layers and 4 pooling layers, and uses residual connections to transfer inter-layer outputs. At the same time, the backbone network also has a CSP structure that realizes forward and cross-stage connection functions;

[0147] The feature pyramid module includes convolutional layers and upsampling operations to process feature maps of different scales; the prediction head module includes convolutional layers and activation functions to generate bounding boxes and category probabilities of targets at different scales;

[0148] The S3.4 is specifically:

[0149] The model input is the original image I, and the normalized input image I is obtained after preprocessing. norm ;

[0150] The preprocessing operations include scaling, normalization, and dedistortion operations to meet the input requirements of the LoFTR model network. The backbone network uses CSPDarknet and CNN (convolutional neural network) structures to transform the input image I norm Convert to a high-dimensional feature map F backbone In the feature pyramid network processing stage, the feature map F extracted by the backbone network is backbone Input into FPN (Feature Pyramid Network), use N to represent the number of layers of the feature pyramid network, and generate a multi-scale feature map F FPN The formula is:

[0151] F FPN ={F level1 , F level2 , ..., F levelN}#(16)

[0152] At each scale F level The prediction head transforms the feature map F level Input into the convolutional layer to generate the bounding box B of the target level and class probability C level , the calculation formula is:

[0153] B level =Sigmoid(Y box )+Grid#(17)

[0154] C level =Softmax(Y conf )#(18)

[0155] In formula (17), Y box is the bounding box coordinate offset of the network output, Sigmoid(Y box ) is the Sigmoid function, Grid is the grid of the anchor box, and in formula (18), Y conf is the category probability score output by the network; finally, the overlapping bounding boxes are filtered out through the non-maximum suppression (NMS) operation, and the targets with higher confidence are retained, and finally the detection results are obtained, including the location and confidence information of the vehicle end;

[0156] For the vehicle end that is successfully detected and tracked, the roadside end uses the calculation method of the roadside coordinate system in S2.2 above to obtain and record the polar coordinates of the vehicle end in the roadside coordinate system, and then generates the vehicle running trajectory based on this.

[0157] S3.5, vehicle-road navigation information fusion: When the roadside binocular camera detects the vehicle, the distance between the left and right binoculars and the vehicle is obtained, and the polar coordinates of the vehicle are obtained according to S2.2 above (D A1 ,θ 1AB ), the exact position of the roadside end in the world coordinate system is known, and the precise coordinates (x, y) of the vehicle end in the world coordinate system are obtained by conversion. The vehicle end uses this coordinate to initialize its own positioning to eliminate the accumulated error.

[0158] The S3.5 is specifically:

[0159] After the vehicle is initialized, it restarts its own trajectory calculation. In the process of leaving the roadside detection, the calculated trajectory is filtered and fused with the vehicle-side running trajectory obtained by the roadside using the polar coordinate calculation method. The particle filter is used as an example to illustrate the vehicle-road navigation information fusion process:

[0160] First, a group of particles are selected from the trajectories calculated by the vehicle-mounted end and the roadside end respectively to represent the possible position of the vehicle in the coordinate system. Then, the steps of updating, resampling, particle fusion, state estimation and state updating are performed according to the following formula, and the final vehicle-road fusion positioning result is obtained after n iterations:

[0161]

[0162] Formula (19) is the motion model update of the vehicle side, represents the position of the i-th vehicle-end particle at time step k+1, represents the position of the i-th vehicle-end particle at time step k, Indicates speed, represents acceleration, Δt represents the time step, represents the system process noise at time step k; formula (20) is the update of the roadside motion model; formula (21) is the measurement update of the vehicle side, represents the weight of the i-th vehicle-end particle at time step k+1, Represents the state of a given particle Under these conditions, the measured value z k+1,vehicle The probability density function of ; Formula (22) is the roadside measurement update; Formula (23) is the calculation of the normalized weight; Formula (24) is the state estimation, represents the state estimate at time step k+1, represents the position of the ith particle at time step k+1; formula (25) is the state update, z k+1 is the most recent measurement.

[0163] Example 1. Campus road scene positioning experiment.

[0164] The campus road was selected as the experimental scene, and the experimental vehicle was used as the experimental carrier. The vehicle was equipped with a set of FLIR Blackfly monocular cameras and an Xsens MTi inertial navigation unit (IMU). At the same time, a road-side device consisting of a ZED binocular camera and a VLP-16 laser rangefinder was set up at a location selected on the road. At the same time, the vehicle-road collaborative positioning system designed by this patent was installed on the vehicle side and the road side respectively. The test environment was the campus of Jiangsu University, and the experimental scenes included: the campus three-way intersection, the road section with more visual occlusion on the road surface, and the road section where the GPS signal was blocked by buildings.

[0165] The specific experiment is as follows: Using the FLIR Blackfly camera, which can take high-resolution images and use the USB3.0 interface, it can provide high-speed data transmission and support real-time image acquisition during vehicle driving. The camera is deployed in the middle of the front windshield of the vehicle. During driving, an image of the front of the vehicle is taken every 1 second. The image is transmitted to the vehicle-side positioning system to calculate the posture change of the vehicle in the previous and next two frames, and then summarized into the vehicle-side coordinate system. Using the Xsens MTi inertial navigation unit (IMU), the device can output information such as the vehicle's posture, acceleration, and angular velocity. It is suitable for the high-precision posture measurement required by this patent. It is deployed at the chassis of the vehicle, as close to the ground as possible. The inertial navigation information measured during the vehicle's driving is transmitted to the vehicle-side positioning system. The posture change of the vehicle is obtained by integration and also summarized into the vehicle-side coordinate system. The two navigation information are used for subsequent fusion to generate positioning results.

[0166] The ZED 2 camera is used, which can take 2K high-resolution photos and supports real-time depth perception. The camera is deployed on the roadside to continuously take pictures and monitor. The photos are transmitted to the roadside positioning system in real time. The positioning system detects the photos to see if there is a vehicle. If not detected, the frame is skipped. If the vehicle to be located is detected, the target detection and tracking of the vehicle to be located is performed, and the vehicle to be located is framed with a target frame for subsequent coordinate measurement. The VLP-16 laser rangefinder is used. The device has 16 laser transmitters and can achieve accurate distance measurement within a radius of 100 meters. It is also deployed on the roadside. When the positioning system detects the vehicle to be located, the device measures the distance between the vehicle and the left and right targets of the camera. The data is used to generate the real-time coordinates of the vehicle in the roadside coordinate system, and then the driving trajectory of the vehicle is summarized to complete the positioning.

[0167] After the preparatory work is completed, the actual application process of vehicle-road collaborative fusion positioning is as follows: the vehicle starts driving, and the vehicle-road positioning system starts working. The vehicle-side positioning system uses visual navigation information and inertial navigation information to complete the vehicle's own positioning in real time. When it drives to the area that the roadside can detect and cover, the roadside reinitializes the vehicle-side positioning system to eliminate the accumulated error. At the same time, a filtering scheme is used to complete the fusion of the multi-sensor positioning data on the vehicle-road side to complete the vehicle-road collaborative positioning.

[0168] Example 2: Underground parking lot road scene positioning experiment.

[0169] The following is a further description of this patent, in which the experimental scenario is an indoor environment in an underground parking lot, where the GPS signal is completely blocked. Under this complex condition, this experiment completely relies on the unique vehicle-road positioning system of this patent to complete the accurate positioning of the vehicle.

[0170] First, in terms of the selection of equipment on the vehicle-road side, this experiment continued the settings of Experimental Example 1, and arranged a roadside device every approximately 20 meters to ensure that the entire underground parking lot scene can be fully covered. When all roadside devices are successfully deployed, the comprehensive integration of vehicle-road data can be achieved, so that the vehicle can obtain accurate positioning information at every moment of driving.

[0171] At the beginning of the experiment, the vehicle-road positioning system was started, and the roadside equipment immediately detected the vehicle to be positioned. Subsequently, the vehicle was initialized and the continuous coordinate changes of the vehicle in the roadside coordinate system were generated. This process not only generated the real-time driving trajectory of the vehicle, but also formed the real-time positioning result of the vehicle by the roadside. At the same time, the vehicle itself used the extended Kalman filter fusion method through the visual and inertial navigation units to generate its own real-time positioning results. Subsequently, the two positioning results of the vehicle-road and vehicle were fused through the particle filter scheme to achieve high-precision positioning of the vehicle.

[0172] This experimental design takes into account the complexity of the underground parking environment and fully utilizes the multi-sensor data fusion technology provided by this patent, ensuring accurate tracking and positioning of the vehicle under extremely challenging conditions. Therefore, this experiment provides strong support and verification for the practical application of this patent in scenarios without GPS signals.

[0173] In this specification, each embodiment is described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same or similar parts between the embodiments can be referred to each other. For the device disclosed in the embodiment, since it corresponds to the method disclosed in the embodiment, the description is relatively simple, and the relevant parts can be referred to the method part.

[0174] The above description of the disclosed embodiments enables one skilled in the art to implement or use the present invention. Various modifications to these embodiments will be apparent to one skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention will not be limited to the embodiments shown herein, but rather to the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A vehicle-road multi-sensor fusion collaborative positioning method based on LoFTR and Yolo algorithm, characterized in that: The following steps are involved: S1. Sensor deployment and navigation data acquisition: including using monocular cameras and binocular cameras on the vehicle side and the roadside side to provide visual navigation information, and using an inertial measurement unit on the vehicle side to provide inertial navigation information; S2. Establishment of positioning coordinate system: The positioning system includes the vehicle-side coordinate system and the roadside coordinate system, and its function is to unify the two to realize the drawing of vehicle driving trajectory; S3, vehicle-road navigation information processing and fusion: The vehicle's own navigation information is integrated with the navigation information formed by the roadside end's detection of the vehicle, forming navigation information with smaller positioning error than the vehicle or roadside end alone, and completing the elimination of the accumulated positioning error of the vehicle end; The S3 is specifically: S3.

1. Vehicle-side visual navigation information processing: The vehicle-side uses the LoFTR method as a visual image processing method. The LoFTR method uses Transformers as the basic model, including feature extraction module, local feature module, coarse matching module and coarse-grained to fine-grained module; The feature extraction module contains a local feature CNN convolutional neural network with an FPN feature pyramid network; the local feature module contains two layers of Transformers, a self-attention layer and a cross-attention layer; the coarse matching module contains the screening of matching confidence, and the softmax activation function is used for classification; the coarse-grained to fine-grained module contains matching refined to the resolution level; S3.2, Vehicle-side inertial navigation information processing: The vehicle side is equipped with an inertial measurement unit as a sensor to measure the linear acceleration and angular velocity of the vehicle; The parameters output by the IMU include the linear acceleration vector a and the angular velocity vector ω, which are used to obtain the position change of the vehicle; S3.3, Fusion of vehicle-side vision and inertial navigation information: During the vehicle's operation time, if the steering angle of the vehicle between two consecutive shooting moments of the monocular camera exceeds 45 degrees, resulting in the visual overlap area between the two moments being less than 50%, the inertial measurement unit data is used instead of the visual data; When the steering angle of the vehicle between two consecutive shots taken by the monocular camera is less than 45 degrees, the extended Kalman filter is used to fuse the information of visual navigation and inertial navigation to estimate the vehicle state. S3.4, roadside visual navigation information processing: the roadside uses a deep learning-based target detection method to detect and track the vehicle captured by the roadside binocular camera. The deep learning-based target detection method has a basic structure that includes three modules: a backbone network, a feature pyramid network, and a prediction head. The backbone network uses CSPDarknet as the basic model, including a lightweight Darknet convolutional network. The convolutional network uses 20 convolutional layers and 4 pooling layers, and uses residual connections to transfer inter-layer outputs. At the same time, the backbone network also has a CSP structure that realizes forward and cross-stage connection functions; The feature pyramid module includes convolutional layers and upsampling operations to process feature maps of different scales; the prediction head module includes convolutional layers and activation functions to generate bounding boxes and category probabilities of targets at different scales; S3.5, vehicle-road navigation information fusion: When the roadside binocular camera detects the vehicle, the distance between the left and right binoculars and the vehicle is obtained, and the polar coordinates of the vehicle are obtained according to S2.2 above (D A1 ,θ 1AB ), the exact position of the roadside end in the world coordinate system is known, and the precise coordinates (x, y) of the vehicle end in the world coordinate system are obtained by conversion. The vehicle end uses this coordinate to initialize its own positioning to eliminate the accumulated error.

2. The vehicle-road multi-sensor fusion collaborative positioning method based on LoFTR and Yolo algorithm according to claim 1 is characterized in that: The S1 is specifically: S1.1: A monocular camera is used on the vehicle side to provide visual navigation information during driving. During driving, the on-board monocular camera captures images in front of the vehicle at fixed intervals, and then the images are pre-processed according to the built-in parameters of the camera. S1.2: Use the inertial measurement unit on the vehicle side to obtain inertial navigation information. When the vehicle is driving, first complete the initial state to obtain the gyroscope angular velocity and vehicle acceleration; S1.3: Deploy a binocular camera at the road end to detect the target information on the vehicle end, and deploy a laser rangefinder to measure the distance information between the vehicle end and the left and right targets of the binocular camera.

3. The vehicle-road multi-sensor fusion collaborative positioning method based on LoFTR and Yolo algorithm according to claim 1 is characterized in that: The S2 is specifically: S2.1, Vehicle-side coordinate system: It is updated in real time during the vehicle's driving process, with the vehicle's current position as the coordinate origin and the current driving direction as the positive half axis of the coordinate system's X-axis; S2.2, Roadside coordinate system: The roadside binocular camera is fixed at a position where the shooting direction is greater than 0° and less than 90° in front of the vehicle's driving direction, with the left eye position of the roadside binocular camera as the coordinate origin, and the direction from the left eye to the right eye of the roadside binocular camera as the positive half axis of the X-axis.

4. The vehicle-road multi-sensor fusion collaborative positioning method based on LoFTR and Yolo algorithm according to claim 3 is characterized in that: The S2.2 is specifically: The formula for calculating the polar coordinates of the vehicle end in the road end coordinate system is as follows: Where: When the roadside detects the vehicle, the roadside laser rangefinder is used to obtain the distance between the vehicle and the left camera on the roadside as D A1 , the distance from the right camera is D B1 , the distance between the left and right cameras is D AB The angle between the line formed by the vehicle end and the roadside end and the positive half axis of the X axis of the roadside coordinate system is θ 1AB .

5. The vehicle-road multi-sensor fusion collaborative positioning method based on LoFTR and Yolo algorithm according to claim 1 is characterized in that: The S3.1 is specifically: The input of the LoFTR model is an image pair I A ,I B , after the feature extraction module, the coarse-grained feature image pair F of 1 / 8 of the original image is extracted A 、F B The fine-grained feature image pair E of 1 / 2 of the original image A 、E B ; The coarse-grained feature image pair passes through the local feature module, is flattened into a 1-D vector, and then adds the position encoding, and then passes through N iterations of the self-attention layer and the cross-attention layer; the coarse-grained feature image pair after iteration passes through the coarse matching module to obtain the confidence matrix P c , and generate a rough matching prediction M c , according to the mutual nearest neighbor criterion (MNN), calculate M c The formula is as follows: Where: The confidence threshold is θ c , the pixel is represented as (i,j), P c (i, j) represents the matching confidence at pixel point (i, j); The rough matching prediction results go through the coarse-grained to fine-grained module. c , a local window of size w×w is cut from the fine-grained feature image pair, and the matching result is refined to the pixel level, and finally the fine matching result M is obtained. f Output; For the feature point pairs in the two images before and after the LoFTR method, the random sample consistency method is used for screening. The specific steps of screening are: First, use three feature point pairs to calculate the fitted plane equation, then bring all feature point pairs into the fitted plane equation model, calculate the point pairs whose errors are within the threshold, called inliers, count the number of inliers, and then iterate the process, constantly compare the number of inliers, and retain the fitting model with the largest number of inliers. The calculation formula for the number of iterations is as follows: In formula (3), k is the number of iterations of the RANSAC method, t is the proportion of inliers in the data, N is the number of feature point pairs, and P is the probability of collecting the correct inliers to calculate the fitting model; The processed feature point pairs are used to realize 3D-2D pose estimation using the PnP problem solving method. Based on the pose estimation, the coordinate changes of the vehicle in the front and back images are calculated to update the vehicle-side coordinate system and draw the vehicle's driving trajectory.

6. The vehicle-road multi-sensor fusion collaborative positioning method based on LoFTR and Yolo algorithm according to claim 1 is characterized in that: The S3.2 is specifically: First, the discrete time series output by the IMU is integrated to obtain the velocity v and position p of the vehicle in the world coordinate system. The calculation formula is as follows: In formula (4), v0 and p0 represent the initial velocity and initial position respectively; Integrate the angular velocity to obtain the vehicle's direction angle θ, which is calculated as follows: θ(t)=∫ω(t)dt+θ0#(5) In formula (5), θ0 represents the initial direction angle; Finally, by fusing the position and orientation of the vehicle, the absolute position and orientation of the vehicle in the world coordinate system is obtained. wb , where T wb is a homogeneous transformation matrix, which represents the transformation from the vehicle coordinate system to the world coordinate system; R(t) is a rotation matrix, and the calculation formula for the absolute position calculated by the vehicle's direction angle is as follows: By obtaining the absolute position and posture in continuous time, the coordinate changes of the vehicle in the previous and next moments are calculated, the vehicle-side coordinate system is updated, and the vehicle's driving trajectory is drawn.

7. The vehicle-road multi-sensor fusion collaborative positioning method based on LoFTR and Yolo algorithm according to claim 1 is characterized in that: The S3.3 is specifically: The state vector x calculated for the visual navigation information vision and the state vector x calculated from the inertial navigation information imu , these two state vectors contain the vehicle's position, speed, and direction in the world coordinate system respectively; the system's state vector X is set to be the concatenation of the two system state vectors: Then the state equation of the system is expressed as: x(k+1)=f(x(k),u(k))+w(k)#(8) In formula (8), f(x(k),u(k)) represents the state transfer function, u(k) represents the external input, and w(k) represents the process noise; the measurement equation is expressed as: z(k)=h(x(k))+v(k)#(9) In formula (9), h(x(k)) represents the measurement function, and v(k) represents the measurement noise; After the system state equation and measurement equation are calculated, for the extended Kalman filter, it is necessary to calculate the state prediction, covariance prediction, Kalman gain, state update and covariance update. The specific formula is as follows: In formula (11), F(k) is the state transfer matrix, Q(k) is the covariance of process noise, and in formula (12), H(k+1) is the measurement matrix, and R(k+1) is the covariance of measurement noise; Through the above five steps, the function of integrating visual and inertial navigation information is realized to obtain a more accurate vehicle state estimation; At the same time, for the problem of large vehicle steering angle, a threshold θ is defined max , represents the maximum threshold of the vehicle steering angle. At each time step k, check whether the vehicle's steering angle θ(k) is greater than θ max ; If θ(k) is greater than the threshold, use Represents a state transfer function based only on IMU data, directly using the IMU state update: Otherwise, continue to use the aforementioned extended Kalman filter for state fusion.

8. The vehicle-road multi-sensor fusion collaborative positioning method based on LoFTR and Yolo algorithm according to claim 1 is characterized in that: The S3.4 is specifically: The model input is the original image I, and the normalized input image I is obtained after preprocessing. norm ; The preprocessing operations include scaling, normalization, and dedistortion operations to meet the input requirements of the LoFTR model network. The backbone network uses CSPDarknet and CNN convolutional neural network structures to transform the input image I norm Convert to a high-dimensional feature map F backbone In the feature pyramid network processing stage, the feature map F extracted by the backbone network is backbone Input into the FPN feature pyramid network, use N to represent the number of layers of the feature pyramid network, and generate a multi-scale feature map F FPN The formula is: F FPN ={F level1 ,F level2 ,…,F levelN }#(16) At each scale F level The prediction head transforms the feature map F level Input into the convolutional layer to generate the bounding box B of the target level and class probability C level , the calculation formula is: B level =Sigmoid(Y box )+Grid#((17) C level =Softmax(Y conf )(18) In formula (17), Y box is the bounding box coordinate offset of the network output, Sigmoid(Y box ) is the Sigmoid function, Grid is the grid of the anchor box, and in formula (18), Y conf is the category probability score output by the network; finally, through the non-maximum suppression operation, the overlapping bounding boxes are filtered out and the targets with higher confidence are retained, and finally the detection results are obtained, including the location and confidence information of the vehicle end; For the vehicle end that is successfully detected and tracked, the roadside end uses the calculation method of the roadside coordinate system in S2.2 above to obtain and record the polar coordinates of the vehicle end in the roadside coordinate system, and then generates the vehicle running trajectory based on this.

9. The vehicle-road multi-sensor fusion collaborative positioning method based on LoFTR and Yolo algorithm according to claim 1 is characterized in that: The S3.5 is specifically: After the vehicle is initialized, it restarts its own trajectory calculation. In the process of leaving the roadside detection, the calculated trajectory is filtered and fused with the vehicle-side running trajectory obtained by the roadside using the polar coordinate calculation method. The particle filter is used as an example to illustrate the vehicle-road navigation information fusion process: First, a group of particles are selected from the trajectories calculated by the vehicle-mounted end and the roadside end respectively to represent the possible position of the vehicle in the coordinate system. Then, the steps of updating, resampling, particle fusion, state estimation and state updating are performed according to the following formula, and the final vehicle-road fusion positioning result is obtained after n iterations: Formula (19) is the motion model update of the vehicle side, represents the position of the i-th vehicle-end particle at time step k+1, represents the position of the i-th vehicle-end particle at time step k, Indicates speed, represents acceleration, Δt represents the time step, represents the system process noise at time step k; formula (20) is the update of the roadside motion model; formula (21) is the measurement update of the vehicle side, represents the weight of the i-th vehicle-end particle at time step k+1, Represents the state of a given particle Under these conditions, the measured value z k+1,vehicle The probability density function of ; Formula (22) is the roadside measurement update; Formula (23) is the calculation of the normalized weight; Formula (24) is the state estimation, represents the state estimate at time step k+1, represents the position of the ith particle at time step k+1; formula (25) is the state update, z k+1 is the most recent measurement.

Images