A vehicle position estimation system and method based on Frenet coordinate multi-source data

Abstract

The present invention discloses a vehicle position estimation system and method for unified coordinates of Frenet coordinate multi-source data. The present invention establishes a Frenet coordinate system; obtains the vehicle vibration signal, vehicle three-dimensional point cloud data, vehicle electromagnetic wave data, and vehicle image data at each acquisition moment and wirelessly transmits them to a central server; the central server calculates the vehicle pile number, lane number, and centroid plane coordinates in each coordinate system corresponding to the vehicle perception sensor at each acquisition moment based on the signal, and constructs the vehicle data of the vehicle perception sensor at each acquisition moment and the centroid plane coordinates in each coordinate system; combines the transformation matrix and coordinate transformation to obtain the centroid plane coordinates of each vehicle coordinate system at each acquisition moment converted to the Frenet coordinate system; calculates the unique centroid plane coordinates of the vehicle corresponding to the vehicle perception sensor at each acquisition moment based on the heterogeneous data error. The multi-source data coordinate unification and fusion method constructed by the present invention can effectively improve the accuracy of vehicle position perception.

Description

Technical Field

[0001] The present invention relates to the field of vehicle coordinate conversion and information fusion, and in particular to a vehicle position estimation system and method for coordinate unified Frenet coordinate multi-source data. Background Art

[0002] In the field of intelligent transportation, although a single sensor can perceive traffic information to a certain extent, it is limited by sensor performance and the traffic information obtained has certain limitations. In addition, it is interfered by factors such as weather and environment. The vehicle driving information obtained by the sensor is easily interrupted in time and space, and it is impossible to obtain high-precision vehicle position information at every moment, which seriously restricts the acquisition of high-precision vehicle trajectory data and makes it impossible to deeply explore traffic flow characteristics.

[0003] The key to obtaining more comprehensive and accurate traffic information is the use of multi-source sensor fusion. Currently, sensors used to collect vehicle driving information primarily focus on road and roadside applications, and these sensors primarily include sensors, radar, and video equipment. During the multi-source information fusion process, the raw data obtained by these devices differ in format and coordinate system, making it difficult to integrate and utilize the data from these devices.

[0004] It can be seen that the existing technical difficulty is how to achieve coordinate unification and information fusion between the data obtained by different sensing devices in the entire road area, so as to establish a unified position relationship between vehicles and roads, and vehicles and vehicles, and improve the accuracy of perceived vehicle position information. Summary of the Invention

[0005] To solve the above technical problems, the present invention provides a unified vehicle position estimation system and method based on Frenet coordinate multi-source data. The system converts the vehicle center of mass coordinate data obtained by road vehicle perception sensors and roadside microwave radars, lidars, and video equipment under different road alignments into a unified Frenet coordinate system, thereby establishing the positional relationships between vehicles and between vehicles and roads. By integrating the perception results of multiple devices, the perception accuracy of vehicle position information is improved, which helps to achieve accurate expression of vehicle trajectory information in the entire time domain.

[0006] The technical solution of the system of the present invention is a coordinate unified vehicle position estimation system based on Frenet coordinate multi-source data, comprising:

[0007] Multiple vehicle perception sensors, multiple lidars, multiple microwave radars, and multiple video devices;

[0008] The central server is wirelessly connected to the plurality of vehicle perception sensors in sequence respectively;

[0009] The central server is wirelessly connected to the plurality of laser radars in sequence respectively;

[0010] The central server is wirelessly connected to the plurality of microwave radars in sequence respectively;

[0011] The central server is wirelessly connected to the plurality of video devices in sequence respectively;

[0012] The Frenet coordinate system is established using the road centerline as the reference line of the Frenet coordinate system. When a vehicle passes by on the road, the vehicle vibration signal, vehicle three-dimensional point cloud data, vehicle electromagnetic wave data, and vehicle image data at each acquisition moment are obtained and wirelessly transmitted to the central server. The central server solves the signal to obtain the stake number, lane number, and center of mass plane coordinates of the vehicle corresponding to the vehicle perception sensor at each acquisition moment, and constructs the vehicle data of the vehicle perception sensor at each acquisition moment and the center of mass plane coordinates in each coordinate system. The transformation is performed in combination with the transformation matrix to obtain the center of mass plane coordinates of each vehicle coordinate system converted to the UTM plane coordinate system at each acquisition moment, and further coordinate transformation is performed to obtain the center of mass plane coordinates of each vehicle coordinate system converted to the Frenet coordinate system at each acquisition moment. Based on the center of mass plane coordinates of the vehicle corresponding to the vehicle perception sensor that perceives the vehicle at each acquisition moment expressed in the Frenet coordinate system by multi-source perception devices, the unique center of mass plane coordinates of the vehicle corresponding to the vehicle perception sensor that perceives the vehicle at each acquisition moment are calculated according to the error size of the heterogeneous data.

[0013] The technical solution of the method of the present invention is a coordinate unified vehicle position estimation method based on Frenet coordinate multi-source data, and the specific steps are as follows:

[0014] Step 1: Use the road centerline as the reference line of the Frenet coordinate system to establish the Frenet coordinate system; use the starting point of the road centerline as the origin of the Frenet coordinate system, the road direction as the horizontal axis S of the Frenet coordinate system, and the road lane distribution direction as the vertical axis D of the Frenet coordinate system;

[0015] Step 2: Lay vehicle perception sensors evenly spaced longitudinally along the center lines of all lanes along the entire road. Lay lidar, microwave radar, and video equipment evenly spaced along both sides of the road.

[0016] Step 3: When a vehicle passes by on the road, the vehicle perception sensor obtains the vehicle vibration signal at each collection moment and transmits it wirelessly to the central server. The lidar obtains the vehicle's three-dimensional point cloud data at each collection moment and transmits it wirelessly to the central server. The microwave radar obtains the vehicle's electromagnetic wave data at each collection moment and transmits it wirelessly to the central server. The video device obtains the vehicle's image data and transmits it wirelessly to the central server.

[0017] Step 4: The central server solves the vehicle vibration signal at each acquisition moment to obtain the stake number of the vehicle corresponding to the vehicle perception sensor at each acquisition moment and the lane number of the vehicle corresponding to the vehicle perception sensor at each acquisition moment; solves the vehicle three-dimensional point cloud data at each acquisition moment to obtain the center of mass plane coordinates of the vehicle corresponding to the vehicle perception sensor at each acquisition moment in the lidar coordinate system; solves the vehicle electromagnetic wave data at each acquisition moment to obtain the center of mass plane coordinates of the vehicle corresponding to the vehicle perception sensor at each acquisition moment in the microwave radar coordinate system; solves the vehicle image data at each acquisition moment separately to obtain the center of mass plane coordinates of the vehicle corresponding to the vehicle perception sensor at each acquisition moment in the pixel coordinate system; constructs the vehicle data of the vehicle perception sensor at each acquisition moment and the center of mass plane coordinates of the vehicle in each coordinate system at each acquisition moment;

[0018] Step 5: Convert the vehicle data of the vehicle perception sensor at each acquisition moment to obtain the coordinates of the vehicle in the Frenet coordinate system at each acquisition moment; obtain the perception section range of each vehicle perception sensor that perceives the vehicle at each acquisition moment, and arbitrarily select 4 feature base points within the perception section range of each vehicle perception sensor that perceives the vehicle at each acquisition moment; obtain the plane coordinates of the lidar and microwave radar in the UTM plane coordinate system, obtain the plane coordinates of each feature base point in the UTM plane coordinate system, the plane coordinates in the lidar coordinate system, the plane coordinates in the microwave radar coordinate system, and the plane coordinates in the pixel coordinate system; according to the plane coordinates of any feature base point in the UTM plane coordinate system, the plane coordinates of the lidar and microwave radar in the UTM plane The plane coordinates of the feature base point in the laser radar coordinate system and the microwave radar coordinate system are obtained, and the transformation matrix of the laser radar coordinate system, the microwave radar coordinate system and the UTM plane coordinate system is obtained. The transformation matrix of the pixel coordinate system and the UTM plane coordinate system is obtained by perspective transformation of the four feature base points; the centroid plane coordinates of the vehicle in each coordinate system at each acquisition moment are combined with the transformation matrix of the corresponding coordinate system and the UTM plane coordinate system to obtain the centroid plane coordinates of each coordinate system of the vehicle at each acquisition moment converted to the UTM plane coordinate system; the centroid plane coordinates of each coordinate system of the vehicle at each acquisition moment are converted to the UTM plane coordinate system and transformed to obtain the centroid plane coordinates of each coordinate system of the vehicle at each acquisition moment converted to the Frenet coordinate system;

[0019] Step 6: Based on the center-of-mass plane coordinates of the vehicle corresponding to the vehicle perception sensor that perceives the vehicle at each acquisition moment expressed by the multi-source perception device in the Frenet coordinate system, calculate the unique center-of-mass plane coordinates of the vehicle corresponding to the vehicle perception sensor that perceives the vehicle at each acquisition moment according to the error size of the heterogeneous data;

[0020] Preferably, the coordinates on the horizontal axis S of the Frenet coordinate system in step 1 represent the cumulative distance traveled by the vehicle on the road;

[0021] The coordinate on the vertical axis D of the Frenet coordinate system in step 1 represents the distance the vehicle deviates from the center line of the road;

[0022] Preferably, the central server in step 4 calculates the vehicle vibration signal at each acquisition moment as follows:

[0023] The stake number of the vehicle perception sensor at each acquisition moment is used as the stake number of the vehicle corresponding to the vehicle perception sensor at each acquisition moment, and is defined as:

[0024]

[0025] k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ]

[0026] Among them, l is the number of acquisition moments, K is the number of vehicle perception sensors, and N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID at the kth collection moment j,k The number of the vehicle corresponding to the vehicle perception sensor, j represents N k The sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle;

[0027] The lane number of the vehicle perception sensor at each acquisition moment is used as the lane number of the vehicle corresponding to the vehicle perception sensor at each acquisition moment, and is defined as:

[0028]

[0029] k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ]

[0030] Among them, l is the number of acquisition moments, K is the number of vehicle perception sensors, and N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID at the kth collection moment j,k The lane number of the vehicle corresponding to the vehicle perception sensor, j represents N k The sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle;

[0031] Step 4 solves the vehicle 3D point cloud data at each acquisition moment as follows:

[0032] The central server uses a deep learning 3D target detection algorithm based on laser point cloud to detect the vehicle's 3D point cloud data at each acquisition moment, and obtains the center of mass plane coordinates of the vehicle corresponding to the vehicle perception sensor at each acquisition moment in the lidar coordinate system, which is defined as:

[0033]

[0034] k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ]

[0035] Among them, l is the number of acquisition moments, K is the number of vehicle perception sensors, and N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID at the kth collection moment j,k The center of mass plane coordinates of the vehicle corresponding to each vehicle perception sensor in the lidar coordinate system, Indicates the ID at the kth collection moment j,k The X-axis coordinate of the center of mass plane coordinate of the vehicle corresponding to each vehicle perception sensor in the lidar coordinate system, Indicates the ID at the kth collection moment j,k The Y-axis coordinate of the center of mass plane coordinate of the vehicle corresponding to the vehicle perception sensor in the lidar coordinate system, j represents N k The sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle;

[0036] Step 4 solves the vehicle electromagnetic wave data at each acquisition moment as follows:

[0037] The central server analyzes the electromagnetic waves sensed by the microwave radar at each acquisition moment according to the electromagnetic wave spectrum at each acquisition moment, and obtains the center of mass plane coordinates of the vehicle corresponding to the vehicle perception sensor at each acquisition moment in the microwave radar coordinate system, which is defined as:

[0038]

[0039] k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ]

[0040] Among them, l is the number of acquisition moments, K is the number of vehicle perception sensors, and N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID at the kth collection moment j,k The center of mass plane coordinates of the vehicle corresponding to each vehicle perception sensor in the microwave radar coordinate system, Indicates the ID at the kth collection moment j,k The X-axis coordinate of the center of mass plane of the vehicle corresponding to each vehicle perception sensor in the microwave radar coordinate system, Indicates the ID at the kth collection moment j,k The Y-axis coordinate of the center of mass plane of the vehicle corresponding to the vehicle perception sensor in the microwave radar coordinate system, j represents N k The sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle;

[0041] Step 4 solves the vehicle image data at each acquisition moment separately, as follows:

[0042] The central server uses a deep learning 2D target detection algorithm to detect the vehicle image data at each acquisition moment, and obtains the center of mass plane coordinates of the vehicle corresponding to the vehicle perception sensor at each acquisition moment in the pixel coordinate system, which is defined as:

[0043]

[0044] k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ]

[0045] Among them, l is the number of acquisition moments, K is the number of vehicle perception sensors, and N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID at the kth collection moment j,k The center of mass plane coordinates of the vehicle corresponding to each vehicle perception sensor in the pixel coordinate system, Indicates the ID at the kth collection moment j,k The X-axis coordinate of the center of mass plane coordinate of the vehicle corresponding to each vehicle perception sensor in the pixel coordinate system, Indicates the ID at the kth collection moment j,k The Y-axis coordinate of the center of mass plane coordinate of the vehicle corresponding to the vehicle perception sensor in the pixel coordinate system, j represents N k The sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle;

[0046] Where i=0 represents the vehicle perception sensor, i=1 represents the lidar, i=2 represents the microwave radar, and i=3 represents the video device;

[0047] The vehicle data of the vehicle perception sensor at each acquisition moment in step 4 is defined as:

[0048]

[0049] k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ]

[0050] Among them, l is the number of acquisition moments, K is the number of vehicle perception sensors, and N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID at the kth collection moment j,k Vehicle data corresponding to vehicle perception sensors, j represents N k The sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle;

[0051] The center of mass plane coordinates of the vehicle in each coordinate system at each acquisition moment in step 4 are defined as:

[0052]

[0053] k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ]

[0054] Where i=1 represents lidar, i=2 represents microwave radar, i=3 represents video equipment, l is the number of acquisition moments, K represents the number of vehicle perception sensors, N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID at the kth collection moment j,k The center of mass plane coordinates of the vehicle corresponding to each vehicle perception sensor in the i coordinate system, Indicates the ID at the kth collection moment j,k The X-axis coordinate of the center of mass plane coordinate of the vehicle corresponding to the vehicle perception sensor in the i coordinate system, Indicates the ID at the kth collection moment j,k The Y-axis coordinate of the center of mass plane coordinate of the vehicle corresponding to the vehicle perception sensor in the i coordinate system, j represents N k The sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle;

[0055] Preferably, the vehicle data of the vehicle perception sensor at each acquisition moment is converted in step 5 as follows:

[0056]

[0057] k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ]

[0058] Among them, l is the number of acquisition moments, K is the number of vehicle perception sensors, and N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID at the kth collection moment j,k The S-axis coordinate of the center of mass plane coordinate of the vehicle corresponding to each vehicle perception sensor in the Frenet coordinate system, Indicates the ID at the kth collection moment j,k The D-axis coordinate of the center of mass plane of the vehicle corresponding to each vehicle perception sensor in the Frenet coordinate system, Indicates the ID at the kth collection moment j,k The stake number of the vehicle corresponding to the vehicle perception sensor, α o Indicates the stake number of the origin of the Frenet coordinate system, β k,0,IDj,k Indicates the ID at the kth collection moment j,k The lane number of the vehicle corresponding to the vehicle perception sensor, A represents the total number of one-way lanes on the road, H represents the lane width, and j represents N k The sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle;

[0059] In step 5, the coordinate systems of each vehicle at each collection moment are converted to the centroid plane coordinates in the UTM plane coordinate system, and the centroid plane coordinates of each vehicle coordinate system at each collection moment are obtained by solving the conversion to the Frenet coordinate system, as follows:

[0060] Step 5.1: Obtain the road section type corresponding to each vehicle perception sensor that perceives the vehicle at each collection moment;

[0061] Step 5.2: Establish the road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment;

[0062] The road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment is established as follows:

[0063] The starting point of the road section centerline corresponding to each vehicle perception sensor that perceives the vehicle at each collection moment is used as the origin of the road section coordinate system, the lane distribution direction is used as the u-axis, and the road direction is used as the v-axis to construct the road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each collection moment;

[0064] Step 5.3: Establish a coordinate transformation relationship between the UTM plane coordinate system and the road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment;

[0065] The coordinate conversion relationship between the UTM plane coordinate system and the road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment is established as follows:

[0066] A coordinate transformation relationship between the UTM plane coordinate system and the road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each collection moment is constructed based on the plane coordinates of the origin of the road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each collection moment in the UTM plane coordinate system and the deflection angle between the UTM plane coordinate system and the road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each collection moment;

[0067] Step 5.4: Convert each vehicle coordinate system at each collection moment to the centroid plane coordinates in the UTM plane coordinate system and perform the conversion based on the coordinate conversion relationship to obtain the centroid plane coordinates of each vehicle coordinate system at each collection moment converted to the road segment coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each collection moment;

[0068] Step 5.5: Based on the road segment type corresponding to each vehicle perception sensor that senses the vehicle at each acquisition moment, calculate the longitudinal distance and lateral offset between the vehicle corresponding to each vehicle perception sensor sensed by the lidar, microwave radar, or video equipment at each acquisition moment and the starting point of the road segment in the road segment coordinate system corresponding to each vehicle perception sensor that senses the vehicle at each acquisition moment;

[0069] The calculation of the longitudinal distance and lateral offset between the vehicle corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment and the starting position of the road section in the road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment is as follows:

[0070] If the road segment type corresponding to each vehicle perception sensor that perceives a vehicle at each collection moment is a straight line, then:

[0071] The longitudinal distance between the vehicle corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment and the starting position of the center line of the road section in the road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment is:

[0072]

[0073] k∈[1,l],ID j,k ∈[1,K],j∈[1,Nk ]

[0074] Where i=1 represents lidar, i=2 represents microwave radar, i=3 represents video equipment, l is the number of acquisition moments, K represents the number of vehicle perception sensors, N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID of the kth acquisition moment perceived by device i j,k The vehicle corresponding to the vehicle perception sensor and the ID at the kth acquisition time j,k The longitudinal distance of the starting point of the center line of the road section in the road section coordinate system corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,k The vehicle corresponding to the vehicle perception sensor that perceives the vehicle is converted to the ID at the kth acquisition time in the UTM plane coordinate system. j,k The u-axis coordinate of the center of mass plane coordinate in the road section coordinate system corresponding to the vehicle perception sensor that perceives the vehicle, j represents N k The sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle;

[0075] The lateral offset between the vehicle corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment and the starting position of the center line of the road section in the road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment is:

[0076]

[0077] k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ]

[0078] Where i=1 represents lidar, i=2 represents microwave radar, i=3 represents video equipment, l is the number of acquisition moments, K represents the number of vehicle perception sensors, N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID of the kth acquisition moment perceived by device i j,k The vehicle corresponding to the vehicle perception sensor and the ID at the kth acquisition time j,k The lateral offset of the starting position of the center line of the road section in the road section coordinate system corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,kThe vehicle corresponding to the vehicle perception sensor that perceives the vehicle is converted to the ID at the kth acquisition time in the UTM plane coordinate system. j,k The v-axis coordinate of the center of mass plane coordinate in the road section coordinate system corresponding to the vehicle perception sensor that perceives the vehicle, j represents N k The sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle;

[0079] If the road section type corresponding to each vehicle perception sensor that perceives a vehicle at each collection moment is a circular curve, then:

[0080] The longitudinal distance between the vehicle corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment and the starting position of the center line of the road section in the road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment is:

[0081]

[0082] k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ]

[0083] Where i=1 represents lidar, i=2 represents microwave radar, i=3 represents video equipment, l is the number of acquisition moments, K represents the number of vehicle perception sensors, N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID of the kth acquisition moment perceived by device i j,k The vehicle corresponding to the vehicle perception sensor and the ID at the kth acquisition time j,k The longitudinal distance of the starting point of the center line of the road section in the road section coordinate system corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,k The vehicle corresponding to the vehicle perception sensor that perceives the vehicle is converted to the ID at the kth acquisition time in the UTM plane coordinate system. j,k The v-axis coordinate of the center of mass plane coordinate in the road section coordinate system corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,k The radius of the circular curve section corresponding to the vehicle perception sensor that perceives the vehicle, j represents N k The sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle;

[0084] The lateral offset between the vehicle corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment and the starting position of the center line of the road section in the road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment is:

[0085]

[0086] k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ]

[0087] Where i=1 represents lidar, i=2 represents microwave radar, i=3 represents video equipment, l is the number of acquisition moments, K represents the number of vehicle perception sensors, N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID of the kth acquisition moment perceived by device i j,k The vehicle corresponding to the vehicle perception sensor and the ID at the kth acquisition time j,k The lateral offset of the starting position of the center line of the road section in the road section coordinate system corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,k The vehicle corresponding to the vehicle perception sensor that perceives the vehicle is converted to the ID at the kth acquisition time in the UTM plane coordinate system. j,k The u-axis coordinate of the center of mass plane coordinate in the road section coordinate system corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,k The vehicle corresponding to the vehicle perception sensor that perceives the vehicle is converted to the ID at the kth acquisition time in the UTM plane coordinate system. j,k The v-axis coordinate of the center of mass plane coordinate in the road section coordinate system corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,k The radius of the circular curve section corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,k The deflection direction of the circular curve section corresponding to the vehicle sensor that senses the vehicle, right deviation is 1, left deviation is -1, j represents N k The sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle;

[0088] If the road section type corresponding to each vehicle perception sensor that perceives a vehicle at each collection moment is a transition curve, then:

[0089] The longitudinal distance and lateral offset between the vehicle corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment and the starting position of the center line of the road section in the road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment are solved based on a cyclic search for the shortest distance, specifically as follows:

[0090] Starting from the origin of the road section coordinate system corresponding to each vehicle perception sensor that senses the vehicle at each collection moment, search forward along the center line of the road section at a certain distance step size for the point on the center line of the road section that is closest to the vehicle corresponding to each vehicle perception sensor that senses the vehicle at each collection moment; based on the distance between the search point and the starting point of the first transition curve section or the end point of the second transition curve section and the plane coordinates of the search point in the road section coordinate system, calculate the longitudinal distance and lateral offset between the vehicle corresponding to each vehicle perception sensor that senses the vehicle at each collection moment and the starting point of the center line of the road section in the road section coordinate system corresponding to each vehicle perception sensor that senses the vehicle at each collection moment;

[0091] The method of searching forward along the center line of the road section for the point on the center line of the road section closest to the vehicle corresponding to each vehicle perception sensor that perceives the vehicle at each collection moment is as follows:

[0092] Each time a step is moved, the plane coordinates and the tangent angle of the current search point in the section coordinate system of the transition curve section corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment are calculated. When the tangent line of the current search point is perpendicular to the line connecting the current search point and the center of mass of the vehicle corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment, the search point is the point on the center line of the section closest to the vehicle corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment. The specific calculation formula is as follows:

[0093]

[0094] k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ]

[0095] Among them, l is the number of acquisition moments, K is the number of vehicle perception sensors, and N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID at the kth collection moment j,k The current search point w on the road centerline corresponding to the vehicle perception sensor that perceives the vehicle and the ID at the kth acquisition time j,k The line connecting the vehicle centroid corresponding to the vehicle perception sensor that perceives the vehicle and the ID at the kth acquisition time j,kThe angle between the v-axis of the road coordinate system corresponding to the vehicle perception sensor that perceives the vehicle and the ID j,k The difference between the tangent angles of the road section corresponding to the current search point w on the center line of the road section corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,k The current search point w on the road centerline corresponding to the vehicle perception sensor that perceives the vehicle and the ID at the kth acquisition time j,k The line connecting the vehicle centroid corresponding to the vehicle perception sensor that perceives the vehicle and the ID at the kth acquisition time j,k The angle formed by the v-axis of the road section coordinate system corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,k The tangent angle of the road section corresponding to the current search point w on the center line of the road section corresponding to the vehicle perception sensor that perceives the vehicle, j represents N k The sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle;

[0096] Among them, when When it is equal to or approximately 90°, the corresponding search point w is closest to the vehicle corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment;

[0097] The specific formula for solving the plane coordinates of any search point on the center line of the transition curve section in the road section coordinate system corresponding to the vehicle perception sensor sensing the vehicle at each acquisition moment is as follows:

[0098]

[0099] k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ]

[0100] Among them, l is the number of acquisition moments, K is the number of vehicle perception sensors, and N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID at the kth collection moment j,k The u-axis coordinate of the center of mass plane coordinate of the current search point w on the transition curve section corresponding to the vehicle perception sensor that perceives the vehicle in the transition curve section coordinate system, Indicates the ID at the kth collection moment j,k The v-axis coordinate of the center of mass plane coordinate of the current search point w on the transition curve section corresponding to the vehicle perception sensor that perceives the vehicle in the transition curve section coordinate system, Indicates the ID at the kth collection moment j,kThe current search point w on the transition curve section corresponding to the vehicle perception sensor that perceives the vehicle and the ID k at the kth acquisition time j,k The distance from the starting point of the first transition curve section or the end point of the second transition curve section corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,k The ID of the kth acquisition moment corresponding to the current search point w on the transition curve section corresponding to the vehicle perception sensor that perceives the vehicle j,k The radius of the transition curve section corresponding to the vehicle perception sensor that perceives the vehicle, changes with the position of any point w, Indicates the ID at the kth collection moment j,k The length of the transition curve section corresponding to the vehicle perception sensor that perceives the vehicle, j represents N k The sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle;

[0101] If the transition curve is the first transition curve, then:

[0102] The longitudinal distance between the vehicle corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment and the starting position of the center line of the road section in the road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment is:

[0103]

[0104] k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ]

[0105] Where i=1 represents lidar, i=2 represents microwave radar, i=3 represents video equipment, l is the number of acquisition moments, K represents the number of vehicle perception sensors, N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID of the kth acquisition moment perceived by device i j,k The vehicle corresponding to the vehicle perception sensor and the ID at the kth acquisition time j,k The longitudinal distance of the starting point of the center line of the road section in the road section coordinate system corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,k The current search point w on the transition curve section corresponding to the vehicle perception sensor that perceives the vehicle and the ID k at the kth acquisition time j,kThe distance from the starting point of the first transition curve section corresponding to the vehicle perception sensor that perceives the vehicle, j represents N k The sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle;

[0106] The lateral offset between the vehicle corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment and the starting position of the center line of the road section in the road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment is:

[0107]

[0108] k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ]

[0109] Where i=1 represents lidar, i=2 represents microwave radar, i=3 represents video equipment, l is the number of acquisition moments, K represents the number of vehicle perception sensors, N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID of the kth acquisition moment perceived by device i j,k The vehicle corresponding to the vehicle perception sensor and the ID at the kth acquisition time j,k The lateral offset of the starting position of the center line of the road section in the road section coordinate system corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,k The vehicle corresponding to the vehicle perception sensor that perceives the vehicle is converted to the ID at the kth acquisition time in the UTM plane coordinate system. j,k The u-axis coordinate of the center of mass plane coordinate in the road section coordinate system corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,k The vehicle corresponding to the vehicle perception sensor that perceives the vehicle is converted to the ID at the kth acquisition time in the UTM plane coordinate system. j,k The v-axis coordinate of the center of mass plane coordinate in the road section coordinate system corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,k The u-axis coordinate of the center of mass plane coordinate of the current search point w on the first transition curve section corresponding to the vehicle perception sensor that perceives the vehicle in the transition curve section coordinate system, Indicates the ID at the kth collection moment j,kThe v-axis coordinate of the center of mass plane coordinate of the current search point w on the first transition curve section corresponding to the vehicle perception sensor that perceives the vehicle in the transition curve section coordinate system, Indicates the ID at the kth collection moment j,k The deflection direction of the first gentle curve section corresponding to the vehicle sensor that senses the vehicle, right deviation is 1, left deviation is -1, j represents N k The sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle;

[0110] If the transition curve is the second transition curve, then:

[0111] The longitudinal distance between the vehicle corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment and the starting position of the center line of the road section in the road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment is:

[0112]

[0113] k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ]

[0114] Where i=1 represents lidar, i=2 represents microwave radar, i=3 represents video equipment, l is the number of acquisition moments, K represents the number of vehicle perception sensors, N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID of the kth acquisition moment perceived by device i j,k The vehicle corresponding to the vehicle perception sensor and the ID at the kth acquisition time j,k The longitudinal distance of the starting point of the center line of the road section in the road section coordinate system corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,k The length of the second transition curve section corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,k The current search point w on the second transition curve section corresponding to the vehicle perception sensor that perceives the vehicle and the ID k at the kth acquisition time j,k The distance from the starting point of the second transition curve section corresponding to the vehicle perception sensor that perceives the vehicle, j represents N k The sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle;

[0115] The lateral offset between the vehicle corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment and the starting position of the center line of the road section in the road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment is:

[0116]

[0117] k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ]

[0118] Where i=1 represents lidar, i=2 represents microwave radar, i=3 represents video equipment, l is the number of acquisition moments, K represents the number of vehicle perception sensors, N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID of the kth acquisition moment perceived by device i j,k The vehicle corresponding to the vehicle perception sensor and the ID at the kth acquisition time j,k The lateral offset of the starting position of the center line of the road section in the road section coordinate system corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,k The vehicle corresponding to the vehicle perception sensor that perceives the vehicle is converted to the ID at the kth acquisition time in the UTM plane coordinate system. j,k The u-axis coordinate of the center of mass plane coordinate in the road section coordinate system corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,k The vehicle corresponding to the vehicle perception sensor that perceives the vehicle is converted to the ID at the kth acquisition time in the UTM plane coordinate system. j,k The v-axis coordinate of the center of mass plane coordinate in the road section coordinate system corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,k The u-axis coordinate of the center of mass plane coordinate of the current search point w on the transition curve section corresponding to the vehicle perception sensor that perceives the vehicle in the transition curve section coordinate system, Indicates the ID at the kth collection moment j,k The v-axis coordinate of the center of mass plane coordinate of the current search point w on the transition curve section corresponding to the vehicle perception sensor that perceives the vehicle in the transition curve section coordinate system, Indicates the ID at the kth collection moment j,k The deflection direction of the second gentle curve section corresponding to the vehicle sensor that senses the vehicle, right deviation is 1, left deviation is -1, j represents N kThe sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle;

[0119] Step 5.6: Calculate the centroid plane coordinates of each vehicle's coordinate system at each acquisition moment, converted from each vehicle's coordinate system to the Frenet coordinate system, by combining the longitudinal distance and lateral offset between the vehicle corresponding to each vehicle perception sensor that sensed the vehicle at each acquisition moment and the starting point of the road section centerline in the road section coordinate system corresponding to each vehicle perception sensor that sensed the vehicle at each acquisition moment.

[0120] The calculation of the center of mass plane coordinates of each vehicle coordinate system converted to the Frenet coordinate system at each acquisition moment is as follows:

[0121]

[0122] k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ]

[0123] Where i=1 represents lidar, i=2 represents microwave radar, i=3 represents video equipment, l is the number of acquisition moments, K represents the number of vehicle perception sensors, N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID of the kth acquisition moment perceived by device i j,k The S-axis coordinate of the center of mass plane coordinate of the vehicle corresponding to each vehicle perception sensor in the Frenet coordinate system, Indicates the ID of the kth acquisition moment perceived by device i j,k The D-axis coordinate of the center of mass plane of the vehicle corresponding to each vehicle perception sensor in the Frenet coordinate system, Indicates the ID of the kth acquisition moment perceived by device i j,k The vehicle corresponding to the vehicle perception sensor and the ID at the kth acquisition time j,k The longitudinal distance of the starting point of the center line of the road section in the road section coordinate system corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID of the kth acquisition moment perceived by device i j,k The vehicle corresponding to the vehicle perception sensor and the ID at the kth acquisition time j,k The lateral offset of the starting position of the center line of the road section in the road section coordinate system corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,k The longitudinal distance between the starting point of the road section centerline corresponding to the vehicle perception sensor that perceives the vehicle and the origin of the Frenet coordinate system, j represents Nk The sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle;

[0124] Preferably, the step 6 is as follows:

[0125] Step 6.1: Use the center-of-mass plane coordinates of the vehicle in the Frenet coordinate system obtained by the vehicle perception sensor at each acquisition moment as the fusion reference, and calculate the difference between the center-of-mass plane coordinates of the vehicle in the Frenet coordinate system obtained by the vehicle perception sensor at each acquisition moment and the center-of-mass plane coordinates of the vehicle in the Frenet coordinate system corresponding to the vehicle perception sensor at each acquisition moment obtained by the lidar, microwave radar, and video equipment;

[0126] Step 6.2: Calculate the final center of mass plane coordinates of the vehicle in the Frenet coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment, as follows:

[0127] When the absolute values of the differences between the center-of-mass plane coordinates of the vehicle in the Frenet coordinate system acquired by the vehicle perception sensor that perceives the vehicle at each acquisition moment and the center-of-mass plane coordinates of the vehicle corresponding to the vehicle perception sensor that perceives the vehicle at each acquisition moment acquired by the laser radar, microwave radar, and video equipment on the S-axis and D-axis are all less than a set threshold range, the center-of-mass plane coordinates of the vehicle corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment acquired by the vehicle perception sensor, laser radar, microwave radar, and video equipment in the Frenet coordinate system are weighted averaged as the final center-of-mass plane coordinates of the vehicle corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment;

[0128] When the absolute value of the difference between the center-of-mass plane coordinates of the vehicle in the Frenet coordinate system acquired by the vehicle perception sensor that perceives the vehicle at each acquisition moment and the center-of-mass plane coordinates of the vehicle in the Frenet coordinate system corresponding to the vehicle perception sensor that perceives the vehicle at each acquisition moment acquired by the laser radar, microwave radar, and video equipment on the S-axis and D-axis is greater than a set threshold range, the center-of-mass plane coordinates of the vehicle in the Frenet coordinate system corresponding to the vehicle perception sensor that perceives the vehicle at each acquisition moment acquired by the vehicle perception sensor, laser radar, microwave radar, and video equipment are optimized, and one of them is retained as the final center-of-mass plane coordinates of the vehicle corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment;

[0129] The centroid plane coordinates of the vehicle in the Frenet coordinate system corresponding to the vehicle perception sensor that perceives the vehicle at each acquisition moment obtained by the vehicle perception sensor, laser radar, microwave radar, and video equipment are optimized. The optimization principle is as follows:

[0130] According to the perception characteristics and accuracy of vehicle perception sensors, lidar, microwave radar, and video equipment, it is preferred to retain the center of mass plane coordinates of the vehicle in the Frenet coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment obtained by the microwave radar as the final center of mass plane coordinates of the vehicle corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment. The center of mass plane coordinate data of the vehicle in the Frenet coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment obtained by the video equipment / lidar and vehicle perception sensors are postponed in sequence. Among them, the preferred order of the center of mass plane coordinate data of the vehicle in the Frenet coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment obtained by the video equipment and lidar is determined according to the vehicle detection accuracy.

[0131] The collected full-time domain multi-source perception vehicle position data is traversed through steps 4 to 6 to obtain full-time domain high-precision vehicle driving trajectory point data.

[0132] The beneficial effects produced by the present invention are:

[0133] The present invention proposes a method for establishing a unified Frenet coordinate system across the entire road area. This method unifies the vehicle centroid coordinates sensed by road vehicle perception sensors, roadside microwave radars, roadside lidars, and roadside video equipment in various coordinate systems across different road alignments into the Frenet coordinate system. This method achieves spatial coordinate consistency of multi-source sensor perception data, thereby obtaining the positional relationships between vehicles and between vehicles and roads.

[0134] The present invention proposes a multi-source sensor perception data fusion method to solve the problems of low accuracy and limited acquisition range of single sensor data. It can significantly improve the accuracy and reliability of vehicle position information extraction and help to extract high-precision vehicle trajectory data over a long time domain. BRIEF DESCRIPTION OF THE DRAWINGS

[0135] Figure 1 : A flow chart of a method according to an embodiment of the present invention;

[0136] Figure 2 : A schematic diagram of a unified Frenet coordinate system for the entire road area according to an embodiment of the present invention;

[0137] Figure 3: Schematic diagram of converting the centroid plane coordinates of a vehicle on the first transition curve section in the UTM plane coordinate system into Frenet coordinates in an embodiment of the present invention. DETAILED DESCRIPTION

[0138] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. 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 making creative efforts are within the scope of protection of the present invention.

[0139] In specific implementation, the method proposed in the technical solution of the present invention can be automatically run by those skilled in the art using computer software technology. System devices that implement the method, such as computer-readable storage media that store the corresponding computer program of the technical solution of the present invention and computer equipment that runs the corresponding computer program, should also be within the scope of protection of the present invention.

[0140] The technical solution of the system of the embodiment of the present invention is a coordinate unified vehicle position estimation system based on Frenet coordinate multi-source data, including:

[0141] Multiple vehicle perception sensors, multiple lidars, multiple microwave radars, and multiple video devices;

[0142] The central server is wirelessly connected to the plurality of vehicle perception sensors in sequence respectively;

[0143] The central server is wirelessly connected to the plurality of laser radars in sequence respectively;

[0144] The central server is wirelessly connected to the plurality of microwave radars in sequence respectively;

[0145] The central server is wirelessly connected to the plurality of video devices in sequence respectively;

[0146] The Frenet coordinate system is established using the road centerline as the reference line of the Frenet coordinate system. When a vehicle passes by on the road, the vehicle vibration signal, vehicle three-dimensional point cloud data, vehicle electromagnetic wave data, and vehicle image data at each acquisition moment are obtained and wirelessly transmitted to the central server. The central server solves the signal to obtain the stake number, lane number, and center of mass plane coordinates of the vehicle corresponding to the vehicle perception sensor at each acquisition moment, and constructs the vehicle data of the vehicle perception sensor at each acquisition moment and the center of mass plane coordinates in each coordinate system. The transformation is performed in combination with the transformation matrix to obtain the center of mass plane coordinates of each vehicle coordinate system converted to the UTM plane coordinate system at each acquisition moment, and further coordinate transformation is performed to obtain the center of mass plane coordinates of each vehicle coordinate system converted to the Frenet coordinate system at each acquisition moment. Based on the center of mass plane coordinates of the vehicle corresponding to the vehicle perception sensor that perceives the vehicle at each acquisition moment expressed in the Frenet coordinate system by multi-source perception devices, the unique center of mass plane coordinates of the vehicle corresponding to the vehicle perception sensor that perceives the vehicle at each acquisition moment are calculated according to the error size of the heterogeneous data.

[0147] The vehicle perception sensor is a G.653 optical fiber.

[0148] The model of the laser radar is Leishen Intelligent CH128x1;

[0149] The model of the microwave radar is Huiershi wide-area radar microwave detector DTAM D39;

[0150] The model of the video device is DS-2DE4423IW-D;

[0151] The following combination Figures 1 to 3 The technical solution of the method of the embodiment of the present invention is a method for estimating vehicle position using Frenet coordinate multi-source data, as follows:

[0152] like Figure 1 As shown, an embodiment of the present invention provides an information fusion method for unifying multi-source data coordinates based on Frenet coordinates, comprising the following steps:

[0153] Step 1: If Figure 2 As shown in the figure, the road centerline is used as the reference line of the Frenet coordinate system to establish the Frenet coordinate system; the starting point of the road centerline is used as the origin of the Frenet coordinate system, the direction along the road is used as the horizontal axis S of the Frenet coordinate system, and the direction of the road lane distribution is used as the vertical axis D of the Frenet coordinate system;

[0154] The coordinates on the horizontal axis S of the Frenet coordinate system in step 1 represent the cumulative distance traveled by the vehicle on the road;

[0155] The coordinate on the vertical axis D of the Frenet coordinate system in step 1 represents the distance the vehicle deviates from the center line of the road;

[0156] Step 2: Install vehicle perception sensors at uniform intervals of 5 meters along the centerline of each lane throughout the entire road. Install lidar sensors at uniform intervals of 200 meters, microwave radars at uniform intervals of 1 km, and video equipment at uniform intervals of 200 meters on both sides of the road.

[0157] Step 3: When a vehicle passes by on the road, the vehicle perception sensor obtains the vehicle vibration signal at each collection moment and transmits it wirelessly to the central server. The lidar obtains the vehicle's three-dimensional point cloud data at each collection moment and transmits it wirelessly to the central server. The microwave radar obtains the vehicle's electromagnetic wave data at each collection moment and transmits it wirelessly to the central server. The video device obtains the vehicle's image data and transmits it wirelessly to the central server.

[0158] Step 4: The central server solves the vehicle vibration signal at each acquisition moment to obtain the stake number of the vehicle corresponding to the vehicle perception sensor at each acquisition moment and the lane number of the vehicle corresponding to the vehicle perception sensor at each acquisition moment; solves the vehicle three-dimensional point cloud data at each acquisition moment to obtain the center of mass plane coordinates of the vehicle corresponding to the vehicle perception sensor at each acquisition moment in the lidar coordinate system; solves the vehicle electromagnetic wave data at each acquisition moment to obtain the center of mass plane coordinates of the vehicle corresponding to the vehicle perception sensor at each acquisition moment in the microwave radar coordinate system; solves the vehicle image data at each acquisition moment separately to obtain the center of mass plane coordinates of the vehicle corresponding to the vehicle perception sensor at each acquisition moment in the pixel coordinate system; constructs the vehicle data of the vehicle perception sensor at each acquisition moment and the center of mass plane coordinates of the vehicle in each coordinate system at each acquisition moment;

[0159] In step 4, the central server calculates the vehicle vibration signal at each acquisition moment as follows:

[0160] The stake number of the vehicle perception sensor at each acquisition moment is used as the stake number of the vehicle corresponding to the vehicle perception sensor at each acquisition moment, and is defined as:

[0161]

[0162] k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ]

[0163] Among them, l is the number of acquisition moments, K is the number of vehicle perception sensors, and N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID at the kth collection moment j,k The number of the vehicle corresponding to the vehicle perception sensor, j represents N k The sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle;

[0164] The lane number of the vehicle perception sensor at each acquisition moment is used as the lane number of the vehicle corresponding to the vehicle perception sensor at each acquisition moment, and is defined as:

[0165]

[0166] k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ]

[0167] Among them, l is the number of acquisition moments, K is the number of vehicle perception sensors, and N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID at the kth collection moment j,k The lane number of the vehicle corresponding to the vehicle perception sensor, j represents N k The sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle;

[0168] Step 4 solves the vehicle 3D point cloud data at each acquisition moment as follows:

[0169] The central server uses a deep learning 3D target detection algorithm based on laser point cloud to detect the vehicle's 3D point cloud data at each acquisition moment, and obtains the center of mass plane coordinates of the vehicle corresponding to the vehicle perception sensor at each acquisition moment in the lidar coordinate system, which is defined as:

[0170]

[0171] k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ]

[0172] Among them, l is the number of acquisition moments, K is the number of vehicle perception sensors, and N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID at the kth collection moment j,k The center of mass plane coordinates of the vehicle corresponding to each vehicle perception sensor in the lidar coordinate system, Indicates the ID at the kth collection moment j,kThe X-axis coordinate of the center of mass plane coordinate of the vehicle corresponding to each vehicle perception sensor in the lidar coordinate system, Indicates the ID at the kth collection moment j,k The Y-axis coordinate of the center of mass plane coordinate of the vehicle corresponding to the vehicle perception sensor in the lidar coordinate system, j represents N k The sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle;

[0173] Step 4 solves the vehicle electromagnetic wave data at each acquisition moment as follows:

[0174] The central server analyzes the electromagnetic waves sensed by the microwave radar at each acquisition moment according to the electromagnetic wave spectrum at each acquisition moment, and obtains the center of mass plane coordinates of the vehicle corresponding to the vehicle perception sensor at each acquisition moment in the microwave radar coordinate system, which is defined as:

[0175]

[0176] k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ]

[0177] Among them, l is the number of acquisition moments, K is the number of vehicle perception sensors, and N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID at the kth collection moment j,k The center of mass plane coordinates of the vehicle corresponding to each vehicle perception sensor in the microwave radar coordinate system, Indicates the ID at the kth collection moment j,k The X-axis coordinate of the center of mass plane of the vehicle corresponding to each vehicle perception sensor in the microwave radar coordinate system, Indicates the ID at the kth collection moment j,k The Y-axis coordinate of the center of mass plane of the vehicle corresponding to the vehicle perception sensor in the microwave radar coordinate system, j represents N k The sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle;

[0178] Step 4 solves the vehicle image data at each acquisition moment separately, as follows:

[0179] The central server uses a deep learning 2D target detection algorithm to detect the vehicle image data at each acquisition moment, and obtains the center of mass plane coordinates of the vehicle corresponding to the vehicle perception sensor at each acquisition moment in the pixel coordinate system, which is defined as:

[0180]

[0181] k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ]

[0182] Among them, l is the number of acquisition moments, K is the number of vehicle perception sensors, and N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID at the kth collection moment j,k The center of mass plane coordinates of the vehicle corresponding to each vehicle perception sensor in the pixel coordinate system, Indicates the ID at the kth collection moment j,k The X-axis coordinate of the center of mass plane coordinate of the vehicle corresponding to each vehicle perception sensor in the pixel coordinate system, Indicates the ID at the kth collection moment j,k The Y-axis coordinate of the center of mass plane coordinate of the vehicle corresponding to the vehicle perception sensor in the pixel coordinate system, j represents N k The sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle;

[0183] Where i=0 represents the vehicle perception sensor, i=1 represents the lidar, i=2 represents the microwave radar, and i=3 represents the video device;

[0184] The vehicle data of the vehicle perception sensor at each acquisition moment in step 4 is defined as:

[0185]

[0186] k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ]

[0187] Among them, l is the number of acquisition moments, K is the number of vehicle perception sensors, and N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID at the kth collection moment j,k Vehicle data corresponding to vehicle perception sensors, j represents N k The sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle;

[0188] The center of mass plane coordinates of the vehicle in each coordinate system at each acquisition moment in step 4 are defined as:

[0189]

[0190] k∈[1,l],IDj,k ∈[1,K],j∈[1,N k ]

[0191] Where i=1 represents lidar, i=2 represents microwave radar, i=3 represents video equipment, l is the number of acquisition moments, K represents the number of vehicle perception sensors, N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID at the kth collection moment j,k The center of mass plane coordinates of the vehicle corresponding to each vehicle perception sensor in the i coordinate system, Indicates the ID at the kth collection moment j,k The X-axis coordinate of the center of mass plane coordinate of the vehicle corresponding to the vehicle perception sensor in the i coordinate system, Indicates the ID at the kth collection moment j,k The Y-axis coordinate of the center of mass plane coordinate of the vehicle corresponding to the vehicle perception sensor in the i coordinate system, j represents N k The sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle;

[0192] Step 5: Convert the vehicle data of the vehicle perception sensor at each acquisition moment to obtain the coordinates of the vehicle in the Frenet coordinate system at each acquisition moment; obtain the perception section range of each vehicle perception sensor that perceives the vehicle at each acquisition moment, and arbitrarily select 4 feature base points within the perception section range of each vehicle perception sensor that perceives the vehicle at each acquisition moment; obtain the plane coordinates of the lidar and microwave radar in the UTM plane coordinate system, obtain the plane coordinates of each feature base point in the UTM plane coordinate system, the plane coordinates in the lidar coordinate system, the plane coordinates in the microwave radar coordinate system, and the plane coordinates in the pixel coordinate system; according to the plane coordinates of any feature base point in the UTM plane coordinate system, the plane coordinates of the lidar and microwave radar in the UTM plane The plane coordinates of the feature base point in the laser radar coordinate system and the microwave radar coordinate system are obtained, and the transformation matrix of the laser radar coordinate system, the microwave radar coordinate system and the UTM plane coordinate system is obtained. The transformation matrix of the pixel coordinate system and the UTM plane coordinate system is obtained by perspective transformation of the four feature base points; the centroid plane coordinates of the vehicle in each coordinate system at each acquisition moment are combined with the transformation matrix of the corresponding coordinate system and the UTM plane coordinate system to obtain the centroid plane coordinates of each coordinate system of the vehicle at each acquisition moment converted to the UTM plane coordinate system; the centroid plane coordinates of each coordinate system of the vehicle at each acquisition moment are converted to the UTM plane coordinate system and transformed to obtain the centroid plane coordinates of each coordinate system of the vehicle at each acquisition moment converted to the Frenet coordinate system;

[0193] The vehicle data of the vehicle perception sensor at each acquisition moment is converted as described in step 5, as follows:

[0194]

[0195] k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ]

[0196] Among them, l is the number of acquisition moments, K is the number of vehicle perception sensors, and N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID at the kth collection moment j,k The S-axis coordinate of the center of mass plane coordinate of the vehicle corresponding to each vehicle perception sensor in the Frenet coordinate system, Indicates the ID at the kth collection moment j,k The D-axis coordinate of the center of mass plane of the vehicle corresponding to each vehicle perception sensor in the Frenet coordinate system, Indicates the ID at the kth collection moment j,k The stake number of the vehicle corresponding to the vehicle perception sensor, α o Indicates the stake number where the origin of the Frenet coordinate system is located. Indicates the ID at the kth collection moment j,k The lane number of the vehicle corresponding to the vehicle perception sensor, A represents the total number of one-way lanes on the road, H represents the lane width, and j represents N k The sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle;

[0197] In step 5, the coordinate systems of each vehicle at each collection moment are converted to the centroid plane coordinates in the UTM plane coordinate system, and the centroid plane coordinates of each vehicle coordinate system at each collection moment are obtained by solving the conversion to the Frenet coordinate system, as follows:

[0198] Step 5.1: Obtain the road section type corresponding to each vehicle perception sensor that perceives the vehicle at each collection moment;

[0199] Step 5.2: Establish the road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment;

[0200] The road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment is established as follows:

[0201] The starting point of the road section centerline corresponding to each vehicle perception sensor that perceives the vehicle at each collection moment is used as the origin of the road section coordinate system, the lane distribution direction is used as the u-axis, and the road direction is used as the v-axis to construct the road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each collection moment;

[0202] Step 5.3: Establish the coordinate transformation relationship between the UTM plane coordinate system and the road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment, as follows:

[0203] The coordinate conversion relationship between the UTM plane coordinate system and the road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment is established as follows:

[0204] A coordinate transformation relationship between the UTM plane coordinate system and the road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each collection moment is constructed based on the plane coordinates of the origin of the road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each collection moment in the UTM plane coordinate system and the deflection angle between the UTM plane coordinate system and the road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each collection moment;

[0205] Step 5.4: Convert each vehicle coordinate system at each collection moment to the centroid plane coordinates in the UTM plane coordinate system and perform the conversion based on the coordinate conversion relationship to obtain the centroid plane coordinates of each vehicle coordinate system at each collection moment converted to the road segment coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each collection moment;

[0206] Step 5.5: Based on the road segment type corresponding to each vehicle perception sensor that senses the vehicle at each acquisition moment, calculate the longitudinal distance and lateral offset between the vehicle corresponding to each vehicle perception sensor sensed by the lidar, microwave radar, or video equipment at each acquisition moment and the starting point of the road segment in the road segment coordinate system corresponding to each vehicle perception sensor that senses the vehicle at each acquisition moment;

[0207] like Figure 3 As shown, when the road section type corresponding to each vehicle perception sensor that perceives a vehicle at each collection moment is the first transition curve, then:

[0208] The longitudinal distance and lateral offset between the vehicle corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment and the starting position of the center line of the road section in the road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment are solved based on a cyclic search for the shortest distance, specifically as follows:

[0209] Starting from the origin of the road section coordinate system corresponding to each vehicle perception sensor that sensed the vehicle at each collection moment, with a search step size of 0.001m, search forward along the center line of the road section for the point on the center line of the road section that is closest to the vehicle corresponding to each vehicle perception sensor that sensed the vehicle at each collection moment; based on the distance between the search point and the starting point of the first transition curve road section and the plane coordinates of the search point in the road section coordinate system, calculate the longitudinal distance and lateral offset between the vehicle corresponding to each vehicle perception sensor that sensed the vehicle at each collection moment and the starting point of the center line of the road section in the road section coordinate system corresponding to each vehicle perception sensor that sensed the vehicle at each collection moment;

[0210] The method of searching forward along the center line of the road section for the point on the center line of the road section closest to the vehicle corresponding to each vehicle perception sensor that perceives the vehicle at each collection moment is as follows:

[0211] Each time a step is moved, the plane coordinates and the tangent angle of the current search point in the first transition curve section corresponding to each vehicle perception sensor that senses the vehicle at each acquisition moment are calculated. When the tangent line of the current search point is perpendicular to the line connecting the current search point and the center of mass of the vehicle corresponding to each vehicle perception sensor that senses the vehicle at each acquisition moment, the search point is the point on the center line of the section closest to the vehicle corresponding to each vehicle perception sensor that senses the vehicle at each acquisition moment. The specific calculation formula is as follows:

[0212]

[0213] k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ]

[0214] Among them, l is the number of acquisition moments, K is the number of vehicle perception sensors, and N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID at the kth collection moment j,k The current search point w on the road centerline corresponding to the vehicle perception sensor that perceives the vehicle and the ID at the kth acquisition time j,k The line connecting the vehicle centroid corresponding to the vehicle perception sensor that perceives the vehicle and the ID at the kth acquisition time j,k The angle between the v-axis of the road coordinate system corresponding to the vehicle perception sensor that perceives the vehicle and the ID j,k The difference between the tangent angles of the road section corresponding to the current search point w on the center line of the road section corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,kThe current search point w on the road centerline corresponding to the vehicle perception sensor that perceives the vehicle and the ID at the kth acquisition time j,k The line connecting the vehicle centroid corresponding to the vehicle perception sensor that perceives the vehicle and the ID at the kth acquisition time j,k The angle formed by the v-axis of the road section coordinate system corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,k The tangent angle of the road section corresponding to the current search point w on the center line of the road section corresponding to the vehicle perception sensor that perceives the vehicle, j represents N k The sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle;

[0215] Among them, when When it is equal to or approximately 90°, the corresponding search point w is closest to the vehicle corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment;

[0216] The specific formula for solving the plane coordinates of the road section coordinate system corresponding to the vehicle perception sensor sensing the vehicle at each acquisition moment at any search point on the center line of the first transition curve section is as follows:

[0217]

[0218] k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ]

[0219] Among them, l is the number of acquisition moments, K is the number of vehicle perception sensors, and N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID at the kth collection moment j,k The u-axis coordinate of the center of mass plane coordinate of the current search point w on the first transition curve section corresponding to the vehicle perception sensor that perceives the vehicle in the first transition curve section coordinate system, Indicates the ID at the kth collection moment j,k The v-axis coordinate of the center of mass plane coordinate of the current search point w on the first transition curve section corresponding to the vehicle perception sensor that perceives the vehicle in the transition curve section coordinate system, Indicates the ID at the kth collection moment j,k The current search point w on the first transition curve section corresponding to the vehicle perception sensor that perceives the vehicle and the ID k at the kth acquisition time j,k The distance from the starting point of the first transition curve section corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,kThe ID number of the kth acquisition moment corresponding to the current search point w on the first transition curve section corresponding to the vehicle perception sensor that perceives the vehicle j,k The radius of the first transition curve section corresponding to the vehicle perception sensor that perceives the vehicle, changes with the position of any point w, Indicates the ID at the kth collection moment j,k The length of the first transition curve section corresponding to the vehicle perception sensor that perceives the vehicle, j represents N k The sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle;

[0220] The longitudinal distance between the vehicle corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment and the starting position of the center line of the road section in the road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment is:

[0221]

[0222] k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ]

[0223] Where i=1 represents lidar, i=2 represents microwave radar, i=3 represents video equipment, l is the number of acquisition moments, K represents the number of vehicle perception sensors, N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID of the kth acquisition moment perceived by device i j,k The vehicle corresponding to the vehicle perception sensor and the ID at the kth acquisition time j,k The longitudinal distance of the starting point of the center line of the road section in the road section coordinate system corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,k The current search point w on the transition curve section corresponding to the vehicle perception sensor that perceives the vehicle and the ID k at the kth acquisition time j,k The distance from the starting point of the first transition curve section corresponding to the vehicle perception sensor that perceives the vehicle, j represents N k The sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle;

[0224] The lateral offset between the vehicle corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment and the starting position of the center line of the road section in the road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment is:

[0225]

[0226] k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ]

[0227] Where i=1 represents lidar, i=2 represents microwave radar, i=3 represents video equipment, l is the number of acquisition moments, K represents the number of vehicle perception sensors, N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID of the kth acquisition moment perceived by device i j,k The vehicle corresponding to the vehicle perception sensor and the ID at the kth acquisition time j,k The lateral offset of the starting position of the center line of the road section in the road section coordinate system corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,k The vehicle corresponding to the vehicle perception sensor that perceives the vehicle is converted to the ID at the kth acquisition time in the UTM plane coordinate system. j,k The u-axis coordinate of the center of mass plane coordinate in the road section coordinate system corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,k The vehicle corresponding to the vehicle perception sensor that perceives the vehicle is converted to the ID at the kth acquisition time in the UTM plane coordinate system. j,k The v-axis coordinate of the center of mass plane coordinate in the road section coordinate system corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,k The u-axis coordinate of the center of mass plane coordinate of the current search point w on the first transition curve section corresponding to the vehicle perception sensor that perceives the vehicle in the transition curve section coordinate system, Indicates the ID at the kth collection moment j,k The v-axis coordinate of the center of mass plane coordinate of the current search point w on the first transition curve section corresponding to the vehicle perception sensor that perceives the vehicle in the transition curve section coordinate system, Indicates the ID at the kth collection moment j,k The deflection direction of the first gentle curve section corresponding to the vehicle sensor that senses the vehicle, right deviation is 1, left deviation is -1, j represents N kThe sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle;

[0228] Step 5.6: Calculate the centroid plane coordinates of each vehicle's coordinate system at each acquisition moment, converted from each vehicle's coordinate system to the Frenet coordinate system, by combining the longitudinal distance and lateral offset between the vehicle corresponding to each vehicle perception sensor that sensed the vehicle at each acquisition moment and the starting point of the road section centerline in the road section coordinate system corresponding to each vehicle perception sensor that sensed the vehicle at each acquisition moment.

[0229] The calculation of the center of mass plane coordinates of each vehicle coordinate system converted to the Frenet coordinate system at each acquisition moment is as follows:

[0230]

[0231] k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ]

[0232] Where i=1 represents lidar, i=2 represents microwave radar, i=3 represents video equipment, l is the number of acquisition moments, K represents the number of vehicle perception sensors, N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID of the kth acquisition moment perceived by device i j,k The S-axis coordinate of the center of mass plane coordinate of the vehicle corresponding to each vehicle perception sensor in the Frenet coordinate system, Indicates the ID of the kth acquisition moment perceived by device i j,k The D-axis coordinate of the center of mass plane of the vehicle corresponding to each vehicle perception sensor in the Frenet coordinate system, Indicates the ID of the kth acquisition moment perceived by device i j,k The vehicle corresponding to the vehicle perception sensor and the ID at the kth acquisition time j,k The longitudinal distance of the starting point of the center line of the road section in the road section coordinate system corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID of the kth acquisition moment perceived by device i j,k The vehicle corresponding to the vehicle perception sensor and the ID at the kth acquisition time j,k The lateral offset of the starting position of the center line of the road section in the road section coordinate system corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,k The longitudinal distance between the starting point of the road section centerline corresponding to the vehicle perception sensor that perceives the vehicle and the origin of the Frenet coordinate system, j represents Nk The sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle;

[0233] Step 6: Based on the center-of-mass plane coordinates of the vehicle corresponding to the vehicle perception sensor that perceives the vehicle at each acquisition moment expressed by the multi-source perception device in the Frenet coordinate system, calculate the unique center-of-mass plane coordinates of the vehicle corresponding to the vehicle perception sensor that perceives the vehicle at each acquisition moment according to the error size of the heterogeneous data;

[0234] Step 6.1: Use the center-of-mass plane coordinates of the vehicle in the Frenet coordinate system obtained by the vehicle perception sensor at each acquisition moment as the fusion reference, and calculate the difference between the center-of-mass plane coordinates of the vehicle in the Frenet coordinate system obtained by the vehicle perception sensor at each acquisition moment and the center-of-mass plane coordinates of the vehicle in the Frenet coordinate system corresponding to the vehicle perception sensor at each acquisition moment obtained by the lidar, microwave radar, and video equipment;

[0235] Step 6.2: Calculate the final center of mass plane coordinates of the vehicle in the Frenet coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment. There are two cases, as follows:

[0236] When the absolute values of the differences between the center-of-mass plane coordinates of the vehicle in the Frenet coordinate system acquired by the vehicle perception sensor that perceives the vehicle at each acquisition moment and the center-of-mass plane coordinates of the vehicle corresponding to the vehicle perception sensor that perceives the vehicle at each acquisition moment acquired by the laser radar, microwave radar, and video equipment on the S-axis and D-axis are all less than a set threshold range, the center-of-mass plane coordinates of the vehicle corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment acquired by the vehicle perception sensor, laser radar, microwave radar, and video equipment in the Frenet coordinate system are weighted averaged as the final center-of-mass plane coordinates of the vehicle corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment;

[0237] When the absolute value of the difference between the center-of-mass plane coordinates of the vehicle in the Frenet coordinate system acquired by the vehicle perception sensor that perceives the vehicle at each acquisition moment and the center-of-mass plane coordinates of the vehicle in the Frenet coordinate system corresponding to the vehicle perception sensor that perceives the vehicle at each acquisition moment acquired by the laser radar, microwave radar, and video equipment on the S-axis and D-axis is greater than a set threshold range, the center-of-mass plane coordinates of the vehicle in the Frenet coordinate system corresponding to the vehicle perception sensor that perceives the vehicle at each acquisition moment acquired by the vehicle perception sensor, laser radar, microwave radar, and video equipment are optimized, and one of them is retained as the final center-of-mass plane coordinates of the vehicle corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment;

[0238] The centroid plane coordinates of the vehicle in the Frenet coordinate system corresponding to the vehicle perception sensor that perceives the vehicle at each acquisition moment obtained by the vehicle perception sensor, laser radar, microwave radar, and video equipment are optimized. The optimization principle is as follows:

[0239] According to the perception characteristics and accuracy of vehicle perception sensors, lidar, microwave radar, and video equipment, it is preferred to retain the center of mass plane coordinates of the vehicle in the Frenet coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment obtained by the microwave radar as the final center of mass plane coordinates of the vehicle corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment. The center of mass plane coordinate data of the vehicle in the Frenet coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment obtained by the video equipment / lidar and vehicle perception sensors are postponed in sequence. Among them, the preferred order of the center of mass plane coordinate data of the vehicle in the Frenet coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment obtained by the video equipment and lidar is determined according to the vehicle detection accuracy.

[0240] The collected full-time domain multi-source perception vehicle position data is traversed through steps 4 to 6 to obtain full-time domain high-precision vehicle driving trajectory point data.

[0241] In summary, the present invention converts the collected road vehicle perception sensor data and roadside microwave radar, lidar, and video equipment perception data in different coordinate systems into the Frenet coordinate system, thereby achieving coordinate unification of multi-source data and obtaining the positional relationship between vehicles and between vehicles and roads; through the fusion of multi-source perception data, it solves the problem that a single sensor cannot accurately obtain vehicle trajectory point information in the entire time domain, thereby improving the accuracy of vehicle position information extraction.

[0242] It should be pointed out that, according to the needs of implementation, the various steps / components described in this application can be split into more steps / components, or two or more steps / components or partial operations of steps / components can be combined into new steps / components to achieve the purpose of the present invention.

[0243] While this invention frequently uses terms such as vehicle perception sensors, laser radars, microwave radars, video equipment, and central servers, evenly spaced along both sides of the road, the use of other terms is not excluded. These terms are used solely to more conveniently describe and explain the essence of the invention; interpreting them as any additional limitations would be contrary to the spirit of the invention.

[0244] It should be understood that the above description of the preferred embodiment is relatively detailed and cannot be regarded as limiting the scope of protection of the patent of the present invention. Under the guidance of the present invention, ordinary technicians in this field can also make substitutions or modifications without departing from the scope of protection of the claims of the present invention, which all fall within the scope of protection of the present invention. The scope of protection requested by the present invention shall be based on the attached claims.

Claims

1. A vehicle position estimation system based on Frenet coordinate multi-source data, characterized in that: include: Multiple vehicle perception sensors, multiple lidars, multiple microwave radars, and multiple video devices; The central server is wirelessly connected to the plurality of vehicle perception sensors in sequence; The central server is wirelessly connected to the plurality of laser radars in sequence respectively; The central server is wirelessly connected to the plurality of microwave radars in sequence respectively; The central server is wirelessly connected to the plurality of video devices in sequence respectively; The Frenet coordinate system is established using the road centerline as the reference line of the Frenet coordinate system. When a vehicle passes by on the road, the vehicle vibration signal, vehicle three-dimensional point cloud data, vehicle electromagnetic wave data, and vehicle image data at each acquisition moment are obtained and wirelessly transmitted to the central server. The central server solves the signal to obtain the stake number, lane number, and center of mass plane coordinates of the vehicle corresponding to the vehicle perception sensor at each acquisition moment, and constructs the vehicle data of the vehicle perception sensor at each acquisition moment and the center of mass plane coordinates in each coordinate system. The transformation is performed in combination with the transformation matrix to obtain the center of mass plane coordinates of each vehicle coordinate system converted to the UTM plane coordinate system at each acquisition moment, and further coordinate transformation is performed to obtain the center of mass plane coordinates of each vehicle coordinate system converted to the Frenet coordinate system at each acquisition moment. Based on the center of mass plane coordinates of the vehicle corresponding to the vehicle perception sensor that perceives the vehicle at each acquisition moment expressed in the Frenet coordinate system by multi-source perception devices, the unique center of mass plane coordinates of the vehicle corresponding to the vehicle perception sensor that perceives the vehicle at each acquisition moment are calculated according to the error size of the heterogeneous data.

2. A method for estimating the coordinate unified vehicle position of Frenet coordinate multi-source data using the Frenet coordinate multi-source data coordinate unified vehicle position estimation system according to claim 1, characterized in that: The following steps are involved: Step 1: Use the road centerline as the reference line of the Frenet coordinate system to establish the Frenet coordinate system; Step 2: Lay vehicle perception sensors evenly spaced longitudinally along the center lines of all lanes along the entire road. Lay lidar, microwave radar, and video equipment evenly spaced along both sides of the road. Step 3: When a vehicle passes by on the road, the vehicle perception sensor obtains the vehicle vibration signal at each collection moment and transmits it wirelessly to the central server. The lidar obtains the vehicle's three-dimensional point cloud data at each collection moment and transmits it wirelessly to the central server. The microwave radar obtains the vehicle's electromagnetic wave data at each collection moment and transmits it wirelessly to the central server. The video device obtains the vehicle's image data and transmits it wirelessly to the central server. Step 4: The central server calculates the signals from the vehicle perception sensors, lidar, microwave radar, and video equipment at each acquisition moment, and obtains the vehicle's stake number, lane number, and center of mass plane coordinates of the vehicle corresponding to the vehicle perception sensor at each acquisition moment. This server then constructs the vehicle data from the vehicle perception sensors at each acquisition moment and the center of mass plane coordinates of the vehicle in each coordinate system at each acquisition moment. Step 5: Convert the vehicle data of the vehicle perception sensor at each acquisition moment to obtain the coordinates of the vehicle in the Frenet coordinate system at each acquisition moment; obtain the transformation matrices of the lidar coordinate system, microwave radar coordinate system, pixel coordinate system and UTM plane coordinate system respectively through projection transformation in combination with the characteristic base points; transform the centroid plane coordinates of the vehicle in each coordinate system at each acquisition moment in combination with the transformation matrix of the corresponding coordinate system and the UTM plane coordinate system to obtain the centroid plane coordinates of each vehicle coordinate system converted to the UTM plane coordinate system at each acquisition moment; transform the centroid plane coordinates of each vehicle coordinate system converted to the UTM plane coordinate system at each acquisition moment to obtain the centroid plane coordinates of each vehicle coordinate system converted to the Frenet coordinate system at each acquisition moment; Step 6: Based on the center-of-mass plane coordinates of the vehicle corresponding to the vehicle perception sensor that perceives the vehicle at each acquisition moment expressed by the multi-source perception device in the Frenet coordinate system, calculate the unique center-of-mass plane coordinates of the vehicle corresponding to the vehicle perception sensor that perceives the vehicle at each acquisition moment according to the error size of the heterogeneous data.

3. The method for estimating vehicle position using Frenet coordinate multi-source data according to claim 2, characterized in that: The Frenet coordinate system is established as described in step 1, as follows: The starting point of the road centerline is used as the origin of the Frenet coordinate system, the road direction is used as the horizontal axis S of the Frenet coordinate system, and the road lane distribution direction is used as the vertical axis D of the Frenet coordinate system; The coordinates on the horizontal axis S of the Frenet coordinate system represent the cumulative distance traveled by the vehicle on the road; The coordinate on the vertical axis D of the Frenet coordinate system represents the distance the vehicle deviates from the center line of the road.

4. The method for estimating vehicle position using Frenet coordinate multi-source data according to claim 3, characterized in that: In step 4, the signals of the vehicle perception sensor, lidar, microwave radar, and video equipment at each acquisition moment are respectively resolved to obtain the vehicle's stake number, lane number, and center of mass plane coordinates of the vehicle in each coordinate system corresponding to the vehicle perception sensor at each acquisition moment, as follows: The vehicle vibration signal at each acquisition moment is solved to obtain the pile number of the vehicle corresponding to the vehicle perception sensor at each acquisition moment and the lane number of the vehicle corresponding to the vehicle perception sensor at each acquisition moment; the vehicle three-dimensional point cloud data at each acquisition moment is solved to obtain the center of mass plane coordinates of the vehicle corresponding to the vehicle perception sensor at each acquisition moment in the lidar coordinate system; the vehicle electromagnetic wave data at each acquisition moment is solved to obtain the center of mass plane coordinates of the vehicle corresponding to the vehicle perception sensor at each acquisition moment in the microwave radar coordinate system; the vehicle image data at each acquisition moment is solved separately to obtain the center of mass plane coordinates of the vehicle corresponding to the vehicle perception sensor at each acquisition moment in the pixel coordinate system.

5. The method for estimating vehicle position using Frenet coordinate multi-source data according to claim 4, characterized in that: In step 4, the central server calculates the vehicle vibration signal at each acquisition moment as follows: The stake number of the vehicle perception sensor at each acquisition moment is used as the stake number of the vehicle corresponding to the vehicle perception sensor at each acquisition moment, and is defined as: k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ] Among them, l is the number of acquisition moments, K is the number of vehicle perception sensors, and N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID at the kth collection moment j,k The number of the vehicle corresponding to the vehicle perception sensor, j represents N k The sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle; The lane number of the vehicle perception sensor at each acquisition moment is used as the lane number of the vehicle corresponding to the vehicle perception sensor at each acquisition moment, and is defined as: k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ] Among them, l is the number of acquisition moments, K is the number of vehicle perception sensors, and N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID at the kth collection moment j,k The lane number of the vehicle corresponding to the vehicle perception sensor, j represents N k The sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle; Step 4 solves the vehicle 3D point cloud data at each acquisition moment as follows: The central server uses a deep learning 3D target detection algorithm based on laser point cloud to detect the vehicle's 3D point cloud data at each acquisition moment, and obtains the center of mass plane coordinates of the vehicle corresponding to the vehicle perception sensor at each acquisition moment in the lidar coordinate system, which is defined as: k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ] Among them, l is the number of acquisition moments, K is the number of vehicle perception sensors, and N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID at the kth collection moment j,k The center of mass plane coordinates of the vehicle corresponding to each vehicle perception sensor in the lidar coordinate system, Indicates the ID at the kth collection moment j,k The X-axis coordinate of the center of mass plane coordinate of the vehicle corresponding to each vehicle perception sensor in the lidar coordinate system, Indicates the ID at the kth collection moment j,k The Y-axis coordinate of the center of mass plane coordinate of the vehicle corresponding to the vehicle perception sensor in the lidar coordinate system, j represents N k The sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle; Step 4 solves the vehicle electromagnetic wave data at each acquisition moment as follows: The central server analyzes the electromagnetic waves sensed by the microwave radar at each acquisition moment according to the electromagnetic wave spectrum at each acquisition moment, and obtains the center of mass plane coordinates of the vehicle corresponding to the vehicle perception sensor at each acquisition moment in the microwave radar coordinate system, which is defined as: k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ] Among them, l is the number of acquisition moments, K is the number of vehicle perception sensors, and N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID at the kth collection moment j,k The center of mass plane coordinates of the vehicle corresponding to each vehicle perception sensor in the microwave radar coordinate system, Indicates the ID at the kth collection moment j,k The X-axis coordinate of the center of mass plane of the vehicle corresponding to each vehicle perception sensor in the microwave radar coordinate system, Indicates the ID at the kth collection moment j,k The Y-axis coordinate of the center of mass plane of the vehicle corresponding to the vehicle perception sensor in the microwave radar coordinate system, j represents N k The sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle; Step 4 solves the vehicle image data at each acquisition moment separately, as follows: The central server uses a deep learning 2D target detection algorithm to detect the vehicle image data at each acquisition moment, and obtains the center of mass plane coordinates of the vehicle corresponding to the vehicle perception sensor at each acquisition moment in the pixel coordinate system, which is defined as: k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ] Among them, l is the number of acquisition moments, K is the number of vehicle perception sensors, and N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID at the kth collection moment j,k The center of mass plane coordinates of the vehicle corresponding to each vehicle perception sensor in the pixel coordinate system, Indicates the ID at the kth collection moment j,k The X-axis coordinate of the center of mass plane coordinate of the vehicle corresponding to each vehicle perception sensor in the pixel coordinate system, Indicates the ID at the kth collection moment j,k The Y-axis coordinate of the center of mass plane coordinate of the vehicle corresponding to the vehicle perception sensor in the pixel coordinate system, j represents N k The sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle; Among them, i=0 represents the vehicle perception sensor, i=1 represents the lidar, i=2 represents the microwave radar, and i=3 represents the video equipment.

6. The method for estimating vehicle position using Frenet coordinate multi-source data according to claim 5, characterized in that: The vehicle data of the vehicle perception sensor at each acquisition moment in step 4 is defined as: k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ] Among them, l is the number of acquisition moments, K is the number of vehicle perception sensors, and N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID at the kth collection moment j,k Vehicle data corresponding to vehicle perception sensors, j represents N k The sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle; The center of mass plane coordinates of the vehicle in each coordinate system at each acquisition moment in step 4 are defined as: k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ] Where i=1 represents lidar, i=2 represents microwave radar, i=3 represents video equipment, l is the number of acquisition moments, K represents the number of vehicle perception sensors, N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID at the kth collection moment j,k The center of mass plane coordinates of the vehicle corresponding to each vehicle perception sensor in the i coordinate system, Indicates the ID at the kth collection moment j,k The X-axis coordinate of the center of mass plane coordinate of the vehicle corresponding to the vehicle perception sensor in the i coordinate system, Indicates the ID at the kth collection moment j,k The Y-axis coordinate of the center of mass plane coordinate of the vehicle corresponding to the vehicle perception sensor in the i coordinate system, j represents N k The serial number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle.

7. The method for estimating vehicle position using Frenet coordinate multi-source data according to claim 6, characterized in that: In step 5, the transformation matrices of the laser radar coordinate system, microwave radar coordinate system, pixel coordinate system and UTM plane coordinate system are obtained by projecting the feature base points, as follows: Obtain the sensing road section range of each vehicle sensing sensor that senses the vehicle at each acquisition moment, and arbitrarily select four feature base points within the sensing road section range of each vehicle sensing sensor that senses the vehicle at each acquisition moment; Obtain the plane coordinates of the lidar and microwave radar in the UTM plane coordinate system, obtain the plane coordinates of each feature base point in the UTM plane coordinate system, the plane coordinates in the lidar coordinate system, the plane coordinates in the microwave radar coordinate system, and the plane coordinates in the pixel coordinate system; according to the plane coordinates of any feature base point in the UTM plane coordinate system, the plane coordinates of the lidar and microwave radar in the UTM plane coordinate system, and the plane coordinates of the feature base point in the lidar coordinate system and the microwave radar coordinate system, obtain the transformation matrix of the lidar coordinate system, the microwave radar coordinate system and the UTM plane coordinate system, and obtain the transformation matrix of the pixel coordinate system and the UTM plane coordinate system by perspective transformation of the four feature base points.

8. The method for estimating vehicle position using Frenet coordinate multi-source data according to claim 7, characterized in that: The vehicle data of the vehicle perception sensor at each acquisition moment is converted as described in step 5, as follows: k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ] Among them, l is the number of acquisition moments, K is the number of vehicle perception sensors, and N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID at the kth collection moment j,k The S-axis coordinate of the center of mass plane coordinate of the vehicle corresponding to each vehicle perception sensor in the Frenet coordinate system, Indicates the ID at the kth collection moment j,k The D-axis coordinate of the center of mass plane of the vehicle corresponding to each vehicle perception sensor in the Frenet coordinate system, Indicates the ID at the kth collection moment j,k The stake number of the vehicle corresponding to the vehicle perception sensor, α o Indicates the stake number where the origin of the Frenet coordinate system is located. Indicates the ID at the kth collection moment j,k The lane number of the vehicle corresponding to the vehicle perception sensor, A represents the total number of one-way lanes on the road, H represents the lane width, and j represents N k The sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle; In step 5, the coordinate systems of each vehicle at each collection moment are converted to the centroid plane coordinates in the UTM plane coordinate system, and the centroid plane coordinates of each vehicle coordinate system at each collection moment are obtained by solving the conversion to the Frenet coordinate system, as follows: Step 5.1: Obtain the road section type corresponding to each vehicle perception sensor that perceives the vehicle at each collection moment; Step 5.2: Establish the road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment; The establishment of the road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment is as follows: The starting point of the road section centerline corresponding to each vehicle perception sensor that perceives the vehicle at each collection moment is used as the origin of the road section coordinate system, the lane distribution direction is used as the u-axis, and the road direction is used as the v-axis to construct the road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each collection moment; Step 5.3: Establish the coordinate transformation relationship between the UTM plane coordinate system and the road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment, as follows: The coordinate conversion relationship between the UTM plane coordinate system and the road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment is established as follows: A coordinate transformation relationship between the UTM plane coordinate system and the road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each collection moment is constructed based on the plane coordinates of the origin of the road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each collection moment in the UTM plane coordinate system and the deflection angle between the UTM plane coordinate system and the road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each collection moment; Step 5.4: Convert each vehicle coordinate system at each collection moment to the centroid plane coordinates in the UTM plane coordinate system and perform the conversion based on the coordinate conversion relationship to obtain the centroid plane coordinates of each vehicle coordinate system at each collection moment converted to the road segment coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each collection moment; Step 5.5: Based on the road segment type corresponding to each vehicle perception sensor that senses the vehicle at each acquisition moment, calculate the longitudinal distance and lateral offset between the vehicle corresponding to each vehicle perception sensor sensed by the lidar, microwave radar, or video equipment at each acquisition moment and the starting point of the road segment in the road segment coordinate system corresponding to each vehicle perception sensor that senses the vehicle at each acquisition moment; Step 5.6: Combine the longitudinal distance and lateral offset between the vehicle corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment and the starting position of the center line of the road section in the road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment by the lidar, microwave radar, and video equipment, and calculate the center of mass plane coordinates of each vehicle coordinate system converted to the Frenet coordinate system at each acquisition moment.

9. The method for estimating vehicle position using Frenet coordinate multi-source data according to claim 8, characterized in that: Step 5.5 calculates the longitudinal distance and lateral offset between the vehicle corresponding to each vehicle perception sensor that senses the vehicle at each acquisition moment and the starting position of the road section in the road section coordinate system corresponding to each vehicle perception sensor that senses the vehicle at each acquisition moment, as follows: If the road segment type corresponding to each vehicle perception sensor that perceives a vehicle at each collection moment is a straight line, then: The longitudinal distance between the vehicle corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment and the starting position of the center line of the road section in the road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment is: k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ] Where i=1 represents lidar, i=2 represents microwave radar, i=3 represents video equipment, l is the number of acquisition moments, K represents the number of vehicle perception sensors, N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID of the kth acquisition moment perceived by device i j,k The vehicle corresponding to the vehicle perception sensor and the ID at the kth acquisition time j,k The longitudinal distance of the starting point of the center line of the road section in the road section coordinate system corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,k The vehicle corresponding to the vehicle perception sensor that perceives the vehicle is converted to the ID at the kth acquisition time in the UTM plane coordinate system. j,k The u-axis coordinate of the center of mass plane coordinate in the road section coordinate system corresponding to the vehicle perception sensor that perceives the vehicle, j represents N k The sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle; The lateral offset between the vehicle corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment and the starting position of the center line of the road section in the road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment is: k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ] Where i=1 represents lidar, i=2 represents microwave radar, i=3 represents video equipment, l is the number of acquisition moments, K represents the number of vehicle perception sensors, N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID of the kth acquisition moment perceived by device i j,k The vehicle corresponding to the vehicle perception sensor and the ID at the kth acquisition time j,k The lateral offset of the starting position of the center line of the road section in the road section coordinate system corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,k The vehicle corresponding to the vehicle perception sensor that perceives the vehicle is converted to the ID at the kth acquisition time in the UTM plane coordinate system. j,k The v-axis coordinate of the center of mass plane coordinate in the road section coordinate system corresponding to the vehicle perception sensor that perceives the vehicle, j represents N k The sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle; If the road section type corresponding to each vehicle perception sensor that perceives a vehicle at each collection moment is a circular curve, then: The longitudinal distance between the vehicle corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment and the starting position of the center line of the road section in the road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment is: k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ] Where i=1 represents lidar, i=2 represents microwave radar, i=3 represents video equipment, l is the number of acquisition moments, K represents the number of vehicle perception sensors, N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID of the kth acquisition moment perceived by device i j,k The vehicle corresponding to the vehicle perception sensor and the ID at the kth acquisition time j,k The longitudinal distance of the starting point of the center line of the road section in the road section coordinate system corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,k The vehicle corresponding to the vehicle perception sensor that perceives the vehicle is converted to the ID at the kth acquisition time in the UTM plane coordinate system. j,k The v-axis coordinate of the center of mass plane coordinate in the road section coordinate system corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,k The radius of the circular curve section corresponding to the vehicle perception sensor that perceives the vehicle, j represents N k The sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle; The lateral offset between the vehicle corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment and the starting position of the center line of the road section in the road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment is: k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ] Where i=1 represents lidar, i=2 represents microwave radar, i=3 represents video equipment, l is the number of acquisition moments, K represents the number of vehicle perception sensors, N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID of the kth acquisition moment perceived by device i j,k The vehicle corresponding to the vehicle perception sensor and the ID at the kth acquisition time j,k The lateral offset of the starting position of the center line of the road section in the road section coordinate system corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,k The vehicle corresponding to the vehicle perception sensor that perceives the vehicle is converted to the ID at the kth acquisition time in the UTM plane coordinate system. j,k The u-axis coordinate of the center of mass plane coordinate in the road section coordinate system corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,k The vehicle corresponding to the vehicle perception sensor that perceives the vehicle is converted to the ID at the kth acquisition time in the UTM plane coordinate system. j,k The v-axis coordinate of the center of mass plane coordinate in the road section coordinate system corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,k The radius of the circular curve section corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,k The deflection direction of the circular curve section corresponding to the vehicle sensor that senses the vehicle, right deviation is 1, left deviation is -1, j represents N k The sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle; If the road section type corresponding to each vehicle perception sensor that perceives a vehicle at each collection moment is a transition curve, then: The longitudinal distance and lateral offset between the vehicle corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment and the starting position of the center line of the road section in the road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment are solved based on a cyclic search for the shortest distance, specifically as follows: Starting from the origin of the road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each collection moment, search forward along the center line of the road section at a certain distance step size for the point on the center line of the road section that is closest to the vehicle corresponding to each vehicle perception sensor that perceives the vehicle at each collection moment; based on the distance between the search point and the starting point of the first transition curve section or the end point of the second transition curve section and the plane coordinates of the search point in the road section coordinate system, calculate the longitudinal distance and lateral offset between the vehicle corresponding to each vehicle perception sensor that perceives the vehicle at each collection moment perceived by the laser radar, microwave radar, and video equipment and the starting position of the center line of the road section in the road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each collection moment; The method of searching forward along the center line of the road section for the point on the center line of the road section closest to the vehicle corresponding to each vehicle perception sensor that perceives the vehicle at each collection moment is as follows: Each time a step is moved, the plane coordinates and the tangent angle of the current search point in the section coordinate system of the transition curve section corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment are calculated. When the tangent line of the current search point is perpendicular to the line connecting the current search point and the center of mass of the vehicle corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment, the search point is the point on the center line of the section closest to the vehicle corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment. The specific calculation formula is as follows: k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ] Among them, l is the number of acquisition moments, K is the number of vehicle perception sensors, and N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID at the kth collection moment j,k The current search point w on the road centerline corresponding to the vehicle perception sensor that perceives the vehicle and the ID at the kth acquisition time j,k The line connecting the vehicle centroid corresponding to the vehicle perception sensor that perceives the vehicle and the ID at the kth acquisition time j,k The angle between the v-axis of the road coordinate system corresponding to the vehicle perception sensor that perceives the vehicle and the ID j,k The difference between the tangent angles of the road section corresponding to the current search point w on the center line of the road section corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,k The current search point w on the road centerline corresponding to the vehicle perception sensor that perceives the vehicle and the ID at the kth acquisition time j,k The line connecting the vehicle centroid corresponding to the vehicle perception sensor that perceives the vehicle and the ID at the kth acquisition time j,k The angle formed by the v-axis of the road section coordinate system corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,k The tangent angle of the road section corresponding to the current search point w on the center line of the road section corresponding to the vehicle perception sensor that perceives the vehicle, j represents N k The sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle; Among them, when When it is equal to or approximately 90°, the corresponding search point w is closest to the vehicle corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment; The specific formula for solving the plane coordinates of any search point on the center line of the transition curve section in the road section coordinate system corresponding to the vehicle perception sensor sensing the vehicle at each acquisition moment is as follows: k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ] Where i=1 represents lidar, i=2 represents microwave radar, i=3 represents video equipment, l is the number of acquisition moments, K represents the number of vehicle perception sensors, N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID at the kth collection moment j,k The u-axis coordinate of the center of mass plane coordinate of the current search point w on the transition curve section corresponding to the vehicle perception sensor that perceives the vehicle in the transition curve section coordinate system, Indicates the ID at the kth collection moment j,k The v-axis coordinate of the center of mass plane coordinate of the current search point w on the transition curve section corresponding to the vehicle perception sensor that perceives the vehicle in the transition curve section coordinate system, Indicates the ID at the kth collection moment j,k The current search point w on the transition curve section corresponding to the vehicle perception sensor that perceives the vehicle and the ID k at the kth acquisition time j,k The distance from the starting point of the first transition curve section or the end point of the second transition curve section corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,k The ID of the kth acquisition moment corresponding to the current search point w on the transition curve section corresponding to the vehicle perception sensor that perceives the vehicle j,k The radius of the transition curve section corresponding to the vehicle perception sensor that perceives the vehicle, changes with the position of any point w, Indicates the ID at the kth collection moment j,k The length of the transition curve section corresponding to the vehicle perception sensor that perceives the vehicle, j represents N k The sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle; If the transition curve is the first transition curve, then: The longitudinal distance between the vehicle corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment and the starting position of the center line of the road section in the road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment is: k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ] Where i=1 represents lidar, i=2 represents microwave radar, i=3 represents video equipment, l is the number of acquisition moments, K represents the number of vehicle perception sensors, N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID of the kth acquisition moment perceived by device i j,k The vehicle corresponding to the vehicle perception sensor and the ID at the kth acquisition time j,k The longitudinal distance of the starting point of the center line of the road section in the road section coordinate system corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,k The current search point w on the transition curve section corresponding to the vehicle perception sensor that perceives the vehicle and the ID k at the kth acquisition time j,k The distance from the starting point of the first transition curve section corresponding to the vehicle perception sensor that perceives the vehicle, j represents N k The sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle; The lateral offset between the vehicle corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment and the starting position of the center line of the road section in the road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment is: k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ] Where i=1 represents lidar, i=2 represents microwave radar, i=3 represents video equipment, l is the number of acquisition moments, K represents the number of vehicle perception sensors, N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID of the kth acquisition moment perceived by device i j,k The vehicle corresponding to the vehicle perception sensor and the ID at the kth acquisition time j,k The lateral offset of the starting position of the center line of the road section in the road section coordinate system corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,k The vehicle corresponding to the vehicle perception sensor that perceives the vehicle is converted to the ID at the kth acquisition time in the UTM plane coordinate system. j,k The u-axis coordinate of the center of mass plane coordinate in the road section coordinate system corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,k The vehicle corresponding to the vehicle perception sensor that perceives the vehicle is converted to the ID at the kth acquisition time in the UTM plane coordinate system. j,k The v-axis coordinate of the center of mass plane coordinate in the road section coordinate system corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,k The u-axis coordinate of the center of mass plane coordinate of the current search point w on the first transition curve section corresponding to the vehicle perception sensor that perceives the vehicle in the transition curve section coordinate system, Indicates the ID at the kth collection moment j,k The v-axis coordinate of the center of mass plane coordinate of the current search point w on the first transition curve section corresponding to the vehicle perception sensor that perceives the vehicle in the transition curve section coordinate system, Indicates the ID at the kth collection moment j,k The deflection direction of the first gentle curve section corresponding to the vehicle sensor that senses the vehicle, right deviation is 1, left deviation is -1, j represents N k The sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle; If the transition curve is the second transition curve, then: The longitudinal distance between the vehicle corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment and the starting position of the center line of the road section in the road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment is: k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ] Where i=1 represents lidar, i=2 represents microwave radar, i=3 represents video equipment, l is the number of acquisition moments, K represents the number of vehicle perception sensors, N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID of the kth acquisition moment perceived by device i j,k The vehicle corresponding to the vehicle perception sensor and the ID at the kth acquisition time j,k The longitudinal distance of the starting point of the center line of the road section in the road section coordinate system corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,k The length of the second transition curve section corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,k The current search point w on the second transition curve section corresponding to the vehicle perception sensor that perceives the vehicle and the ID k at the kth acquisition time j,k The distance from the starting point of the second transition curve section corresponding to the vehicle perception sensor that perceives the vehicle, j represents N k The sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle; The lateral offset between the vehicle corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment and the starting position of the center line of the road section in the road section coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment is: k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ] Where i=1 represents lidar, i=2 represents microwave radar, i=3 represents video equipment, l is the number of acquisition moments, K represents the number of vehicle perception sensors, N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID of the kth acquisition moment perceived by device i j,k The vehicle corresponding to the vehicle perception sensor and the ID at the kth acquisition time j,k The lateral offset of the starting position of the center line of the road section in the road section coordinate system corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,k The vehicle corresponding to the vehicle perception sensor that perceives the vehicle is converted to the ID at the kth acquisition time in the UTM plane coordinate system. j,k The u-axis coordinate of the center of mass plane coordinate in the road section coordinate system corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,k The vehicle corresponding to the vehicle perception sensor that perceives the vehicle is converted to the ID at the kth acquisition time in the UTM plane coordinate system. j,k The v-axis coordinate of the center of mass plane coordinate in the road section coordinate system corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,k The u-axis coordinate of the center of mass plane coordinate of the current search point w on the transition curve section corresponding to the vehicle perception sensor that perceives the vehicle in the transition curve section coordinate system, Indicates the ID at the kth collection moment j,k The v-axis coordinate of the center of mass plane coordinate of the current search point w on the transition curve section corresponding to the vehicle perception sensor that perceives the vehicle in the transition curve section coordinate system, Indicates the ID at the kth collection moment j,k The deflection direction of the second gentle curve section corresponding to the vehicle sensor that senses the vehicle, right deviation is 1, left deviation is -1, j represents N k The sequence number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle; Step 5.6 calculates the center of mass plane coordinates of each vehicle coordinate system converted to the Frenet coordinate system at each acquisition moment, as follows: k∈[1,l],ID j,k ∈[1,K],j∈[1,N k ] Where i=1 represents lidar, i=2 represents microwave radar, i=3 represents video equipment, l is the number of acquisition moments, K represents the number of vehicle perception sensors, N k represents the number of vehicle perception sensors that perceive the vehicle at the kth acquisition moment, Indicates the ID of the kth acquisition moment perceived by device i j,k The S-axis coordinate of the center of mass plane coordinate of the vehicle corresponding to each vehicle perception sensor in the Frenet coordinate system, Indicates the ID of the kth acquisition moment perceived by device i j,k The D-axis coordinate of the center of mass plane of the vehicle corresponding to each vehicle perception sensor in the Frenet coordinate system, Indicates the ID of the kth acquisition moment perceived by device i j,k The vehicle corresponding to the vehicle perception sensor and the ID at the kth acquisition time j,k The longitudinal distance of the starting point of the center line of the road section in the road section coordinate system corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID of the kth acquisition moment perceived by device i j,k The vehicle corresponding to the vehicle perception sensor and the ID at the kth acquisition time j,k The lateral offset of the starting position of the center line of the road section in the road section coordinate system corresponding to the vehicle perception sensor that perceives the vehicle, Indicates the ID at the kth collection moment j,k The longitudinal distance between the starting point of the road section centerline corresponding to the vehicle perception sensor that perceives the vehicle and the origin of the Frenet coordinate system, j represents N k The serial number of the j-th vehicle perception sensor that perceives the vehicle among the vehicle perception sensors that perceive the vehicle.

10. The method for estimating vehicle position using Frenet coordinate multi-source data according to claim 9, characterized in that: The step 6 is specifically as follows: Step 6.1: Use the center-of-mass plane coordinates of the vehicle in the Frenet coordinate system obtained by the vehicle perception sensor at each acquisition moment as the fusion reference, and calculate the difference between the center-of-mass plane coordinates of the vehicle in the Frenet coordinate system obtained by the vehicle perception sensor at each acquisition moment and the center-of-mass plane coordinates of the vehicle in the Frenet coordinate system corresponding to the vehicle perception sensor at each acquisition moment obtained by the lidar, microwave radar, and video equipment; Step 6.2: Calculate the final center of mass plane coordinates of the vehicle in the Frenet coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment, as follows: When the absolute values of the differences between the center-of-mass plane coordinates of the vehicle in the Frenet coordinate system acquired by the vehicle perception sensor that perceives the vehicle at each acquisition moment and the center-of-mass plane coordinates of the vehicle corresponding to the vehicle perception sensor that perceives the vehicle at each acquisition moment acquired by the laser radar, microwave radar, and video equipment on the S-axis and D-axis are all less than a set threshold range, the center-of-mass plane coordinates of the vehicle corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment acquired by the vehicle perception sensor, laser radar, microwave radar, and video equipment in the Frenet coordinate system are weighted averaged as the final center-of-mass plane coordinates of the vehicle corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment; When the absolute value of the difference between the center-of-mass plane coordinates of the vehicle in the Frenet coordinate system acquired by the vehicle perception sensor that perceives the vehicle at each acquisition moment and the center-of-mass plane coordinates of the vehicle in the Frenet coordinate system corresponding to the vehicle perception sensor that perceives the vehicle at each acquisition moment acquired by the laser radar, microwave radar, and video equipment on the S-axis and D-axis is greater than a set threshold range, the center-of-mass plane coordinates of the vehicle in the Frenet coordinate system corresponding to the vehicle perception sensor that perceives the vehicle at each acquisition moment acquired by the vehicle perception sensor, laser radar, microwave radar, and video equipment are optimized, and one of them is retained as the final center-of-mass plane coordinates of the vehicle corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment; The centroid plane coordinates of the vehicle in the Frenet coordinate system corresponding to the vehicle perception sensor that perceives the vehicle at each acquisition moment obtained by the vehicle perception sensor, laser radar, microwave radar, and video equipment are optimized. The optimization principle is as follows: According to the perception characteristics and accuracy of vehicle perception sensors, lidar, microwave radar, and video equipment, it is preferred to retain the center of mass plane coordinates of the vehicle in the Frenet coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment obtained by the microwave radar as the final center of mass plane coordinates of the vehicle corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment. The center of mass plane coordinate data of the vehicle in the Frenet coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment obtained by the video equipment / lidar and vehicle perception sensors are postponed in sequence. Among them, the preferred order of the center of mass plane coordinate data of the vehicle in the Frenet coordinate system corresponding to each vehicle perception sensor that perceives the vehicle at each acquisition moment obtained by the video equipment and lidar is determined according to the vehicle detection accuracy.

Images