Vehicle positioning method

Abstract

The present disclosure provides a vehicle positioning method, the method comprising: when it is determined according to a real-time positioning state of a vehicle and a high-definition map that there is a track near the vehicle, determining M candidate tracks in a target track area by matching a first track feature in the high-definition map and a second track feature detected by a radar in real time; determining a vehicle positioning state at a second time by using a state estimation algorithm, the vehicle positioning state at a first time and IMU information at the second time, correcting the vehicle positioning state at the second time by using the state estimation algorithm and relative position information of each candidate track in the high-definition map and the vehicle, and obtaining M corrected vehicle positioning states at the second time; determining a target track actually detected by the radar based on a nearest neighbor matching algorithm, the corrected vehicle positioning state at the second time and the relative position information, and taking the corrected vehicle positioning state at the second time calculated using the target track as the vehicle positioning state of the vehicle at the second time.

Description

Technical Field

[0001] This disclosure relates to the field of automotive technology, and more specifically, to a vehicle positioning method. Background Technology

[0002] With the advancement and development of technology, intelligent driving vehicles have been vigorously developed. For intelligent driving vehicles, high-precision positioning capability is an essential function. However, in special scenarios such as ports, relying solely on GPS (Global Positioning System) and / or IMU (Inertial Measurement Unit) for positioning will lead to the following problems: (1) GPS is easily affected by interference from other signals, multipath effects, etc., resulting in poor reliability and robustness; (2) Low-precision IMUs are difficult to perform DR (Dead Reckoning) calculations over long periods of time, while high-precision IMUs are more expensive.

[0003] In conclusion, improving vehicle positioning accuracy in special scenarios such as ports while saving costs is an urgent problem to be solved. Summary of the Invention

[0004] This disclosure provides a vehicle positioning method that can improve vehicle positioning accuracy in special scenarios such as ports while saving costs.

[0005] The specific technical solution is as follows:

[0006] In a first aspect, embodiments of this disclosure provide a vehicle positioning method, the method comprising:

[0007] When it is determined that there is a track near the vehicle based on the vehicle's real-time positioning status and high-precision map, M candidate tracks in the target track area are determined by matching the first track features stored in the high-precision map with the second track features detected by radar in real time, where M is a positive integer;

[0008] The vehicle positioning state at the second moment is determined using a state estimation algorithm, the vehicle positioning state at the first moment, and the inertial navigation sensor (IMU) information at the second moment. The vehicle positioning state at the second moment is then corrected using the state estimation algorithm and the relative position information of each of the M candidate tracks in the high-precision map with respect to the vehicle, resulting in M ​​corrected vehicle positioning states at the second moment. Here, the first moment is the moment preceding the second moment, and the second moment is the moment when the radar detects the second track feature.

[0009] Based on the nearest neighbor matching algorithm, the M corrected vehicle positioning states at the second time, and the relative position information of each candidate track with the vehicle in the high-precision map, the target track actually detected by the radar is determined, and the corrected vehicle positioning state at the second time calculated using the target track is taken as the vehicle positioning state at the second time.

[0010] In one embodiment, the method further includes:

[0011] When the vehicle passes the target track, the longitudinal coordinates of the vehicle's current position are corrected based on the target track, the vehicle's current position in the vehicle's current positioning status, and the real-time measured IMU information, wherein the current position is greater than or equal to the second position.

[0012] In one implementation, when the vehicle passes the target track, the longitudinal coordinate of the vehicle's current positioning position is corrected based on the target track, the vehicle's current positioning position in the vehicle's current positioning status, and real-time measured IMU information. This includes:

[0013] When it is determined that the vehicle has passed the target track based on the real-time measured IMU information, a reference position is determined, wherein the reference position is the intersection of the target track and the vehicle's planned path in the high-precision map;

[0014] If the longitudinal error between the current vehicle positioning position and the reference position is less than the error threshold, the longitudinal coordinates of the current vehicle positioning position are corrected to the longitudinal coordinates of the reference position in the vehicle coordinate system.

[0015] In one implementation, determining that the vehicle has passed the target track based on the real-time measured IMU information includes:

[0016] If the distance between the vehicle's current location on the high-precision map and the target track is less than a first distance threshold, and the error between the measurement variance of the IMU information measured within the target time period and the reference measurement variance is less than an error threshold, then it is determined that the vehicle has passed through the target track.

[0017] In one embodiment, when the measurement variance of the IMU information includes accelerometer measurement variance and gyroscope measurement variance, and the reference measurement variance includes accelerometer reference measurement variance and gyroscope reference measurement variance, the error between the measurement variance of the IMU information measured within the target time period and the reference measurement variance is less than an error threshold, including:

[0018] Within the target time period, the error between the accelerometer measurement variance and the accelerometer reference measurement variance is less than the error threshold, and the error between the gyroscope measurement variance and the gyroscope reference measurement variance is less than the error threshold.

[0019] In one implementation, determining the target track actually detected by the radar based on the nearest neighbor matching algorithm, the M corrected vehicle positioning states at the second time point, and the relative position information of each candidate track with the vehicle in the high-precision map includes:

[0020] Based on the relative position information of each candidate track with the vehicle in the high-precision map, the target distance difference corresponding to each candidate track is determined. The relative position information includes a first distance, a second distance, and a target angle. The first distance is the vertical distance from the vehicle's representative position at the second moment to the candidate track. The vehicle's representative position at the second moment is the corresponding position of the vehicle's positioning position in the high-precision map in the corrected vehicle positioning state at the second moment. The second distance is the longitudinal distance between the vehicle and the candidate track in the vehicle coordinate system actually measured by the radar at the second moment. The target angle is the angle between the driving direction represented by the heading angle in the corrected vehicle positioning state at the second moment and the candidate track in the high-precision map. The target distance difference is the difference between the vertical distance from the vehicle to the candidate track actually measured by the radar and the first distance.

[0021] The target trajectory actually detected by the radar is determined based on the target distance differences corresponding to the M candidate trajectories and the least squares method.

[0022] In one implementation, determining the target distance difference corresponding to each candidate track based on the relative position information of each candidate track with respect to the vehicle in the high-precision map includes:

[0023] The target distance difference corresponding to each candidate trajectory is calculated according to the first formula.

[0024] The first formula includes:

[0025]

[0026] Wherein, z i This represents the target distance difference corresponding to the i-th candidate orbit. The distance represents the second distance corresponding to the i-th candidate trajectory, where θ represents the target angle corresponding to the i-th candidate trajectory, and d i This represents the first distance corresponding to the i-th candidate orbit;

[0027] The first distance d is calculated according to the second formula. i The second formula includes:

[0028]

[0029] Wherein, the x pv The y pv The x and y coordinates represent the vehicle's position at the second time point, respectively. pr The y pr These represent the horizontal and vertical coordinates of the perpendicular point obtained on the i-th candidate track after drawing a perpendicular line from the vehicle's representative position at the second moment to the i-th candidate track in the high-precision map, respectively, and d0 represents the longitudinal installation distance between the radar and the IMU.

[0030] In one implementation, determining the target trajectory actually detected by the radar based on the target distance differences corresponding to the M candidate trajectories and the least squares method includes:

[0031] The j-th candidate orbit is determined as the target orbit according to the third formula.

[0032] The third formula includes:

[0033]

[0034] The z j This represents the target distance difference corresponding to the j-th candidate orbit.

[0035] In one implementation, determining the presence of a track near the vehicle based on the vehicle's real-time positioning status and a high-precision map includes:

[0036] Find the vehicle's location in the real-time positioning status of the vehicle in the high-precision map;

[0037] If the distance between the vehicle's location in the real-time positioning state and the track in the high-precision map is less than a second distance threshold, and the angle between the driving direction represented by the heading angle in the real-time positioning state and the track in the high-precision map is within the target vertical range, then it is determined that the track exists near the vehicle, wherein the target vertical range includes 90 degrees.

[0038] In one implementation, before determining M candidate orbits located within the target orbit region corresponding to the orbit by matching the first orbit features stored in the high-precision map with the second orbit features detected by radar in real time, the method further includes: determining the target orbit region;

[0039] Determining the target orbital region includes:

[0040] The width of the current lane or the width after widening the current lane by the target distance is taken as the lateral width of the target track area;

[0041] The longitudinal length of the target track region is determined based on the vehicle's longitudinal positioning uncertainty at the first moment and the radar's ranging noise.

[0042] Secondly, another embodiment of this disclosure provides a vehicle positioning device, the device comprising:

[0043] The candidate track determination unit is used to determine M candidate tracks within the target track area by matching the first track features stored in the high-precision map with the second track features detected by radar in real time, based on the real-time positioning status of the vehicle and the high-precision map. M is a positive integer.

[0044] The prediction unit is used to determine the vehicle positioning state at the second time using a state estimation algorithm, the vehicle positioning state at the first time, and the inertial navigation sensor IMU information at the second time; and to correct the vehicle positioning state at the second time using the state estimation algorithm and the relative position information of each of the M candidate tracks with the vehicle in the high-precision map, respectively, to obtain M corrected vehicle positioning states at the second time, wherein the first time is the previous time adjacent to the second time, and the second time is the time when the radar detects the second track feature;

[0045] The target trajectory determination unit is used to determine the target trajectory actually detected by the radar based on the nearest neighbor matching algorithm, the vehicle positioning status at the M corrected second time moments, and the relative position information of each candidate trajectory with the vehicle in the high-precision map.

[0046] A positioning state determination unit is used to take the corrected vehicle positioning state at the second moment, calculated using the target track, as the vehicle positioning state at the second moment.

[0047] In one embodiment, the device further includes:

[0048] The correction unit is used to correct the longitudinal coordinate of the vehicle's current position when the vehicle passes the target track, based on the target track, the vehicle's current position in the vehicle's current positioning status, and the real-time measured IMU information, wherein the current time is greater than or equal to the second time.

[0049] In one embodiment, the correction unit includes:

[0050] The reference position determination module is used to determine a reference position when the vehicle passes through the target track based on the real-time measured IMU information, wherein the reference position is the intersection of the target track and the vehicle's planned path in the high-precision map;

[0051] The correction module is used to correct the longitudinal coordinates of the vehicle positioning position at the current moment to the longitudinal coordinates of the reference position in the vehicle body coordinate system when the longitudinal error between the current vehicle positioning position and the reference position is less than the error threshold.

[0052] In one embodiment, the reference position determination module is used to determine that the vehicle has passed through the target track when the distance between the current vehicle positioning position in the high-precision map and the target track is less than a first distance threshold, and the error between the measurement variance of the IMU information measured within the target time period and the reference measurement variance is less than an error threshold, and when the vehicle has passed through the target track, a reference position is determined.

[0053] In one embodiment, a correction module is configured to, when the measurement variance of the IMU information includes accelerometer measurement variance and gyroscope measurement variance, and the reference measurement variance includes accelerometer reference measurement variance and gyroscope reference measurement variance, and within the target time period, the error between the accelerometer measurement variance and the accelerometer reference measurement variance is less than the error threshold, and the error between the gyroscope measurement variance and the gyroscope reference measurement variance is less than the error threshold, correct the longitudinal coordinate of the vehicle positioning position at the current moment to the longitudinal coordinate of the reference position in the vehicle body coordinate system.

[0054] In one embodiment, the target orbit determination unit includes:

[0055] The difference determination module is used to determine the target distance difference corresponding to each candidate track based on the relative position information of each candidate track and the vehicle in the high-precision map. The relative position information includes a first distance, a second distance, and a target angle. The first distance is the vertical distance from the vehicle's representative position at the second moment to the candidate track. The vehicle's representative position at the second moment is the corresponding position of the vehicle's positioning position in the high-precision map in the corrected vehicle positioning state at the second moment. The second distance is the longitudinal distance between the vehicle and the candidate track in the vehicle coordinate system actually measured by the radar at the second moment. The target angle is the angle between the driving direction represented by the heading angle in the corrected vehicle positioning state at the second moment and the candidate track in the high-precision map. The target distance difference is the difference between the vertical distance from the vehicle to the candidate track actually measured by the radar and the first distance.

[0056] The target trajectory determination module is used to determine the target trajectory actually detected by the radar based on the target distance differences corresponding to the M candidate trajectories and the least squares method.

[0057] In one implementation, the difference determination module is used to calculate the target distance difference corresponding to each of the candidate orbitals according to a first formula.

[0058] The first formula includes:

[0059]

[0060] Wherein, z i This represents the target distance difference corresponding to the i-th candidate orbit. The distance represents the second distance corresponding to the i-th candidate trajectory, where θ represents the target angle corresponding to the i-th candidate trajectory, and d i This represents the first distance corresponding to the i-th candidate orbit;

[0061] The difference determination module is used to calculate the first distance d according to the second formula. i The second formula includes:

[0062]

[0063] Wherein, the x pv The y pv The x and y coordinates represent the vehicle's position at the second time point, respectively. pr The y prThese represent the horizontal and vertical coordinates of the perpendicular point obtained on the i-th candidate track after drawing a perpendicular line from the vehicle's representative position at the second moment to the i-th candidate track in the high-precision map, respectively, and d0 represents the longitudinal installation distance between the radar and the IMU.

[0064] In one implementation, the target orbit determination module is used to determine the j-th candidate orbit as the target orbit according to a third formula.

[0065] The third formula includes:

[0066]

[0067] The z j This represents the target distance difference corresponding to the j-th candidate orbit.

[0068] In one embodiment, the device further includes:

[0069] The track detection unit is used to determine whether there is a track near the vehicle based on the vehicle's real-time positioning status and high-precision map.

[0070] The track detection unit includes:

[0071] The search module is used to search for the vehicle's location in the real-time positioning status of the vehicle in the high-precision map;

[0072] The track detection module is used to determine that a track exists near the vehicle when the distance between the vehicle's real-time positioning position and the track in the high-precision map is less than a second distance threshold, and the angle between the driving direction represented by the heading angle in the vehicle's real-time positioning state and the track in the high-precision map is within the target vertical range. The target vertical range includes 90 degrees.

[0073] In one embodiment, the device further includes:

[0074] The region determination unit is used to determine the target orbit region before determining M candidate orbits within the target orbit region by matching the first orbit features stored in the high-precision map with the second orbit features detected by radar in real time;

[0075] The region determination unit includes:

[0076] The lateral width determination module is used to determine the lateral width of the target track area by using the width of the current lane or the width after widening the current lane by a target distance.

[0077] The longitudinal length determination module is used to determine the longitudinal length of the target track area based on the vehicle longitudinal positioning uncertainty at the first moment and the ranging noise of the radar.

[0078] Thirdly, another embodiment of this disclosure provides a storage medium having executable instructions stored thereon, which, when executed by a processor, cause the processor to perform the method as described in any embodiment of the first aspect.

[0079] Fourthly, another embodiment of this disclosure provides a vehicle, the vehicle comprising:

[0080] One or more processors;

[0081] Storage device for storing one or more programs.

[0082] When the one or more programs are executed by the one or more processors, the vehicle performs the method as described in any embodiment of the first aspect.

[0083] As can be seen from the above, the vehicle positioning method provided in this embodiment can, when determining the existence of a track near the vehicle based on the real-time positioning status of the vehicle and a high-precision map, firstly match the first track features stored in the high-precision map with the second track features detected by the radar in real time to determine M candidate tracks within the target track area. Then, it uses a state estimation algorithm, the vehicle positioning status at the first moment, and the IMU information at the second moment to determine the vehicle positioning status at the second moment. Furthermore, it uses the state estimation algorithm and the relative position information of each of the M candidate tracks with respect to the vehicle in the high-precision map to correct the vehicle positioning status at the second moment, obtaining M corrected vehicle positioning states at the second moment, where the first moment is the moment immediately preceding the second moment. Finally, based on the nearest neighbor matching algorithm, the M corrected vehicle positioning states at the second moment, and the relative position information of each candidate track with respect to the vehicle in the high-precision map, it determines the target track actually detected by the radar. Finally, it uses the corrected vehicle positioning state at the second moment calculated using the target track as the vehicle positioning state at the second moment. Therefore, this embodiment of the present disclosure can correct the vehicle's current positioning position based on the relative position information between the candidate track and the vehicle, the state estimation algorithm, and the nearest neighbor matching algorithm when the vehicle detects a track. This improves vehicle positioning accuracy in special scenarios such as ports while saving costs. Furthermore, when the vehicle passes the target track, the longitudinal coordinates of the vehicle's current positioning position are corrected based on the target track, the vehicle's current positioning position, and the real-time measured IMU information, further improving the positioning accuracy when the vehicle passes the target track. Attached Figure Description

[0084] To more clearly illustrate the technical solutions in the embodiments of this disclosure or the prior art, the accompanying drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are merely some embodiments of this disclosure. For those skilled in the art, other drawings can be obtained based on these drawings without any creative effort.

[0085] Figure 1 This is a schematic diagram of the structure of a vehicle positioning system provided in an embodiment of the present disclosure;

[0086] Figure 2 An example diagram illustrating the installation location of some vehicle parts according to an embodiment of this disclosure;

[0087] Figure 3 A schematic flowchart illustrating a vehicle positioning method provided in an embodiment of this disclosure;

[0088] Figure 4 An example diagram of a target orbital region provided in an embodiment of this disclosure;

[0089] Figure 5 This is a schematic flowchart of a method for determining a target trajectory provided in an embodiment of the present disclosure;

[0090] Figure 6 A flowchart illustrating another vehicle positioning method provided in this embodiment of the present disclosure;

[0091] Figure 7 This is a schematic flowchart of a positioning correction method provided in an embodiment of the present disclosure;

[0092] Figure 8 A flowchart illustrating yet another vehicle positioning method provided in this disclosure embodiment;

[0093] Figure 9 This is a block diagram of a vehicle positioning device provided in an embodiment of the present disclosure. Detailed Implementation

[0094] The technical solutions of the embodiments of this disclosure will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of this disclosure, and not all of them. All other embodiments obtained by those skilled in the art based on the embodiments of this disclosure without creative effort are within the scope of protection of this disclosure.

[0095] It should be noted that the terms "comprising" and "having," and any variations thereof, in the embodiments and drawings of this disclosure are intended to cover non-exclusive inclusion. For example, a process, method, system, product, or device that includes a series of steps or units is not limited to the steps or units listed, but may optionally include steps or units not listed, or may optionally include other steps or units inherent to these processes, methods, products, or devices.

[0096] Figure 1 This is a schematic diagram of a vehicle positioning system provided in an embodiment of the present disclosure. The system includes a GNSS (Global Navigation Satellite System) module 11, an IMU 12, a core processing unit 13, a communication module 14, a power supply module 15, a non-volatile memory 16, and a volatile memory 17. The GNSS module 11 is used for satellite positioning. The IMU 12 is used to acquire inertial navigation information, such as vehicle acceleration and angular velocity. The communication module 14 includes CAN (Controller Area Network) communication and Ethernet communication, enabling real-time information exchange with external systems such as millimeter-wave radar 18, lidar 19, a forward-facing camera 110, an ESP (Electronic Stability Program) controller 111, and an EPS (Electric Power Steering) controller 112. The power supply module 15 supplies power to the other modules. The non-volatile memory 16 can store high-precision maps, and the volatile memory 17 can store intermediate data during vehicle operation. The installation positions of the millimeter-wave radar 18, lidar 19, forward-looking camera 110, and IMU 12 can be as follows: Figure 2 As shown. Among them, the millimeter-wave radar 18 includes three, which are installed at the left, center and right positions of the front of the vehicle, respectively; the lidar 19 includes two, which are installed on the left and right sides of the front of the vehicle, respectively; the forward-looking camera 110 is installed in the middle of the front of the vehicle; and the IMU 12 is installed in the middle of the vehicle.

[0097] Figure 3 This is a flowchart illustrating a vehicle positioning method provided in an embodiment of the present disclosure. The method can be applied to vehicles and mainly includes:

[0098] S210: If the vehicle's real-time positioning status and high-precision map indicate the presence of a track near the vehicle, M candidate tracks within the target track area are determined by matching the first track features stored in the high-precision map with the second track features detected by radar in real time.

[0099] Where M is a positive integer. Real-time vehicle positioning status includes the real-time vehicle position and heading angle. The vehicle positioning status covariance includes the covariance matrix of the vehicle position and heading angle, i.e., the diagonal elements represent the variance values ​​of the uncertainty of the vehicle position and heading angle results, and the other elements represent the covariance between the variables. The first track feature is the radar-detected track feature pre-stored in the high-precision map, and the second track feature is the vehicle's real-time radar-detected track feature. The radar detecting the first track feature and the radar detecting the second track feature can be the same radar or different radars. Track features are information extracted from the radar point cloud, such as the relative position, relative speed, angle, and reflection intensity of the track and vehicle. By combining indicators such as the reflection intensity, azimuth, and position of reflective points, more advanced feature indicators can be proposed.

[0100] The specific implementation of determining the presence of a track near a vehicle based on the vehicle's real-time positioning status and a high-precision map includes: locating the vehicle's position in the high-precision map based on its real-time positioning status; determining the presence of a track near the vehicle when the distance between the vehicle's real-time positioning status and the track in the high-precision map is less than a second distance threshold, and the angle between the vehicle's heading angle (representing the driving direction) and the track in the high-precision map is within the target vertical range. The target vertical range includes 90 degrees; that is, when the angle between the driving direction and the track in the high-precision map is 90 degrees or close to 90 degrees, the presence of a track near the vehicle is determined. The second distance threshold can be determined based on practical experience, for example, it can be 100 meters.

[0101] Before determining the M candidate orbits within the target orbit region by matching the first orbit features stored in the high-precision map with the second orbit features detected by radar in real time, it is also necessary to determine the target orbit region itself. For example, Figure 4 As shown, the method for determining the target track area includes: using the width of the current lane or the width after widening the target distance based on the current lane as the lateral width of the target track area; and determining the longitudinal length of the target track area based on the vehicle longitudinal positioning uncertainty at the first moment and the ranging noise of the radar.

[0102] Among them, the vehicle longitudinal positioning uncertainty is the projection of the vehicle positioning variance corresponding to the vehicle positioning state covariance matrix onto the longitudinal axis of the vehicle coordinate system. Radar ranging noise is the variance of radar ranging. For example, when a radar measures an object at a fixed distance, its output value will not be a constant but rather a fluctuating value. The variance of these data, under the assumption of a Gaussian distribution, constitutes the ranging noise.

[0103] It should be added that, when the total number of tracks in the target track area of ​​the high-precision map is N, if N≤M, all tracks in the area can be considered as candidate tracks. If N>M, the first track features stored in the high-precision map can be matched with the second track features detected by radar in real time, and the M tracks with high similarity between the first and second track features can be determined as candidate tracks. Furthermore, the tracks involved in this embodiment can be crane tracks or other types of tracks.

[0104] S220: Determine the vehicle positioning state at the second moment using the state estimation algorithm, the vehicle positioning state at the first moment, and the inertial navigation sensor (IMU) information at the second moment. Then, use the state estimation algorithm and the relative position information of each of the M candidate tracks with the vehicle in the high-precision map to correct the vehicle positioning state at the second moment, and obtain M corrected vehicle positioning states at the second moment.

[0105] The state estimation algorithm can be a Kalman filter algorithm or other optimal estimation algorithms. Kalman filtering is an algorithm that uses the state equations of a linear system to optimally estimate the system state using system input and output observation data. This application embodiment can employ a distributed Kalman filter algorithm. IMU information includes vehicle acceleration, wheel speed, and angular velocity. The first time point is the time preceding the second time point, i.e., the time corresponding to the previous computation cycle of the second time point. The second time point is the time when the radar detects the second track feature. Relative position information includes at least one of a first distance, a second distance, and a target angle. The first distance is the vertical distance from the vehicle's representative position at the first time point to the candidate track; the vehicle's representative position is the corresponding position of the vehicle's positioning state in the high-precision map; the second distance is the longitudinal distance between the vehicle and the candidate track in the vehicle coordinate system, actually measured by the radar at the first time point; and the target angle is the angle between the heading angle (representing the driving direction) in the vehicle's positioning state at the second time point and the candidate track in the high-precision map. In this step, only the first and second distances can be used in the calculation to correct the vehicle's positioning state at the second time point. The correction of the vehicle positioning status at the second moment mainly includes: correcting the longitudinal coordinates of the vehicle positioning position in the vehicle positioning status at the second moment.

[0106] S230: Based on the nearest neighbor matching algorithm, the vehicle positioning status of M corrected second time moments, and the relative position information of each candidate track with the vehicle in the high-precision map, determine the target track actually detected by the radar, and use the corrected vehicle positioning status of the second time moment calculated using the target track as the vehicle positioning status of the vehicle at the second time moment.

[0107] The specific implementation method of this step can be as follows: Figure 5 As shown:

[0108] (S231) Based on the relative position information of each candidate track and the vehicle in the high-precision map, determine the target distance difference corresponding to each candidate track.

[0109] In this step, the target distance can be calculated using the first distance, the second distance, and the included angle from the relative position information.

[0110] The target distance difference for each candidate orbital is calculated using the first formula.

[0111] The first formula includes:

[0112]

[0113] Among them, z i This represents the target distance difference corresponding to the i-th candidate orbit. Let d represent the second distance corresponding to the i-th candidate trajectory, θ represent the target angle corresponding to the i-th candidate trajectory, and d represent the distance between the i-th and i-th candidate trajectories. i This represents the first distance corresponding to the i-th candidate track.

[0114] The first distance is calculated using the second formula, which includes:

[0115]

[0116] Where, d i Let x represent the first distance corresponding to the i-th candidate orbit. pv y pv Let x and y represent the x and y coordinates of the vehicle's position at the second time point, respectively. pr y pr d1 and d2 respectively represent the horizontal and vertical coordinates of the perpendicular point obtained on the i-th candidate track after drawing a perpendicular line from the vehicle's representative position at the second moment to the i-th candidate track in the high-precision map, and d0 represents the longitudinal distance between the radar and the vehicle's positioning point.

[0117] (S232) Determine the target trajectory actually detected by the radar based on the target distance differences corresponding to the M candidate trajectories and the least squares method.

[0118] Based on the third formula, the j-th candidate orbit is determined as the target orbit.

[0119] The third formula includes:

[0120]

[0121] z j This represents the target distance difference corresponding to the j-th candidate orbit.

[0122] The vehicle positioning method provided in this disclosure, when determining the existence of a track near the vehicle based on the vehicle's real-time positioning status and a high-precision map, firstly determines M candidate tracks within the target track area by matching the first track features stored in the high-precision map with the second track features detected by the radar in real time. Then, it determines the vehicle positioning status at the second time using a state estimation algorithm, the vehicle positioning status at the first time, and the IMU information at the second time. Finally, it corrects the vehicle positioning status at the second time using the state estimation algorithm and the relative position information of each of the M candidate tracks with the vehicle in the high-precision map, obtaining M corrected vehicle positioning states at the second time, where the first time is the time immediately preceding the second time. Based on the nearest neighbor matching algorithm, the M corrected vehicle positioning states at the second time, and the relative position information of each candidate track with the vehicle in the high-precision map, it determines the target track actually detected by the radar. Finally, it uses the corrected vehicle positioning status at the second time calculated using the target track as the vehicle positioning status at the second time. Therefore, the embodiments of this disclosure can correct the vehicle's current positioning position based on the relative position information between the candidate track and the vehicle, the state estimation algorithm, and the nearest neighbor matching algorithm when the vehicle detects a track. This can improve the vehicle positioning accuracy in special scenarios such as ports while saving costs.

[0123] In one implementation, Figure 3 Based on this, the embodiments of this disclosure also provide, as Figure 6 The method shown:

[0124] S240: When the vehicle passes the target track, the longitudinal coordinates of the vehicle's current position are corrected based on the target track, the vehicle's current position in the vehicle's current positioning status, and the real-time measured IMU information.

[0125] The current time is greater than or equal to the first time. IMU information includes vehicle acceleration and angular velocity, among other things.

[0126] The specific implementation method of this step can be as follows: Figure 7 As shown:

[0127] (S241) When the vehicle passes through the target track based on the IMU information measured in real time, a reference position is determined, wherein the reference position is the intersection of the target track and the planned path of the vehicle in the high-precision map.

[0128] If the distance between the vehicle's current location on the high-precision map and the target track is less than a first distance threshold, and the error between the measurement variance of the IMU information measured within the target time period and the reference measurement variance is less than an error threshold, then the vehicle is determined to have passed the target track. The first distance threshold and the error threshold are determined based on practical experience; for example, the first distance threshold can be 2 meters, and the error threshold can be 0.05.

[0129] (S242) If the longitudinal error between the current vehicle positioning position and the reference position is less than the error threshold, the longitudinal coordinates in the current vehicle positioning position are corrected to the longitudinal coordinates of the reference position in the vehicle body coordinate system.

[0130] When the measurement variance of the IMU information includes the accelerometer measurement variance and the gyroscope measurement variance, and the reference measurement variance includes the accelerometer reference measurement variance and the gyroscope reference measurement variance, the error between the measurement variance of the IMU information measured within the target time period and the reference measurement variance is less than the error threshold includes: within the target time period, the error between the accelerometer measurement variance and the accelerometer reference measurement variance is less than the error threshold, and the error between the gyroscope measurement variance and the gyroscope reference measurement variance is less than the error threshold. The target time period can be (t, tT), where t is the current time, and T is the duration of the calibration-selected passage through the target orbit.

[0131] The vehicle positioning method provided in this disclosure can correct the longitudinal coordinates of the vehicle's current positioning position based on the target track, the vehicle's current positioning position, and the real-time measured IMU information when the vehicle passes through the target track, thereby further improving the positioning accuracy when the vehicle passes through the target track.

[0132] In one implementation, the specific process of the vehicle from startup to positioning on the track can be as follows: Figure 8 As shown, the method includes:

[0133] S310: Perform system initialization after vehicle startup.

[0134] S320: Diagnose whether the system has malfunctioned.

[0135] S330: Report the fault in the event of a malfunction.

[0136] S340: Obtain the real-time location status of the vehicle in the absence of a malfunction.

[0137] The vehicle's real-time location status is obtained based on a fusion positioning algorithm. This fusion positioning algorithm combines satellite positioning and IMU positioning.

[0138] S350: Determine whether there is a track near the vehicle based on the vehicle's real-time positioning status and high-precision map. If a track exists, proceed to step S360; otherwise, return to step S340.

[0139] S360: By matching the first orbital features stored in the high-precision map with the second orbital features detected by radar in real time, M candidate orbits within the target orbital area are determined.

[0140] S370: Determine the vehicle positioning status at the second time using the state estimation algorithm, the vehicle positioning status at the first time, and the IMU information at the second time. Then, use the state estimation algorithm and the relative position information of each candidate track in the M candidate tracks with the vehicle in the high-precision map to correct the vehicle positioning status at the second time, and obtain M corrected vehicle positioning statuses at the second time.

[0141] S380: Based on the nearest neighbor matching algorithm, the vehicle positioning status of M corrected second time moments, and the relative position information of each candidate track with the vehicle in the high-precision map, the target track actually detected by the radar is determined, and the corrected vehicle positioning status of the second time moment calculated using the target track is used as the vehicle positioning status of the vehicle at the second time moment.

[0142] S390: Continue to obtain the vehicle's real-time location status.

[0143] S3100: Determine whether the distance between the current vehicle location on the high-precision map and the target track is less than a first distance threshold, and whether the error between the measurement variance of the IMU information measured within the target time period and the reference measurement variance is less than an error threshold. If both determinations are yes, proceed to step S3110; if not both determinations are yes, return to step S390.

[0144] S3110: Determine the reference position.

[0145] S3120: Determine whether the longitudinal error between the current vehicle positioning position and the reference position is less than the error threshold. If the determination result is yes, proceed to step S3130; if the determination result is no, return to step S390.

[0146] S3130: Correct the longitudinal coordinates of the current vehicle positioning position to the longitudinal coordinates of the reference position in the vehicle coordinate system.

[0147] Based on the above embodiments, another embodiment of this disclosure provides a vehicle positioning device, such as... Figure 9 As shown, the device includes:

[0148] The candidate track determination unit 40 is used to determine M candidate tracks in the target track area by matching the first track features stored in the high-precision map with the second track features detected by the radar in real time, when it is determined that there are tracks near the vehicle based on the real-time positioning status of the vehicle and the high-precision map. M is a positive integer.

[0149] The prediction unit 42 is used to determine the vehicle positioning state at the second time using a state estimation algorithm, the vehicle positioning state at the first time, and the inertial navigation sensor IMU information at the second time; and to correct the vehicle positioning state at the second time using the state estimation algorithm and the relative position information of each of the M candidate tracks in the high-precision map with the vehicle, respectively, to obtain M corrected vehicle positioning states at the second time, wherein the first time is the previous time adjacent to the second time, and the second time is the time when the radar detects the second track feature;

[0150] The target trajectory determination unit 44 is used to determine the target trajectory actually detected by the radar based on the nearest neighbor matching algorithm, the vehicle positioning status of the M corrected second time moments, and the relative position information of each candidate trajectory with the vehicle in the high-precision map.

[0151] The positioning state determination unit 46 is used to take the corrected vehicle positioning state at the second moment, calculated using the target track, as the vehicle positioning state at the second moment.

[0152] In one embodiment, the device further includes:

[0153] The correction unit is used to correct the longitudinal coordinate of the vehicle's current position when the vehicle passes the target track, based on the target track, the vehicle's current position in the vehicle's current positioning status, and the real-time measured IMU information, wherein the current time is greater than or equal to the second time.

[0154] In one embodiment, the correction unit includes:

[0155] The reference position determination module is used to determine the reference position when the vehicle passes through the target track based on the real-time measured IMU information. The reference position is the intersection of the target track and the vehicle's planned path in the high-precision map.

[0156] The correction module is used to correct the longitudinal coordinates of the vehicle positioning position at the current moment to the longitudinal coordinates of the reference position in the vehicle body coordinate system when the longitudinal error between the current vehicle positioning position and the reference position is less than the error threshold.

[0157] In one implementation, the reference position determination module is used to determine that the vehicle has passed through the target track when the distance between the vehicle's current location in the high-precision map and the target track is less than a first distance threshold, and the error between the measurement variance of the IMU information measured within the target time period and the reference measurement variance is less than an error threshold. When the vehicle has passed through the target track, the module determines the reference position.

[0158] In one embodiment, the correction module is used to correct the longitudinal coordinates of the vehicle positioning position at the current moment to the longitudinal coordinates of the reference position in the vehicle body coordinate system when the measurement variance of the IMU information includes the accelerometer measurement variance and the gyroscope measurement variance, and the reference measurement variance includes the accelerometer reference measurement variance and the gyroscope reference measurement variance, and within the target time period, the error between the accelerometer measurement variance and the accelerometer reference measurement variance is less than an error threshold, and the error between the gyroscope measurement variance and the gyroscope reference measurement variance is less than an error threshold.

[0159] In one embodiment, the target trajectory determination unit 44 includes:

[0160] The difference determination module is used to determine the target distance difference corresponding to each candidate track based on the relative position information of each candidate track and the vehicle in the high-precision map. The relative position information includes a first distance, a second distance, and a target angle. The first distance is the vertical distance from the vehicle's representative position at the second moment to the candidate track. The vehicle's representative position at the second moment is the corresponding position of the vehicle's positioning position in the high-precision map in the corrected vehicle positioning state at the second moment. The second distance is the longitudinal distance between the vehicle and the candidate track in the vehicle coordinate system actually measured by the radar at the second moment. The target angle is the angle between the driving direction represented by the heading angle in the corrected vehicle positioning state at the second moment and the candidate track in the high-precision map. The target distance difference is the difference between the vertical distance from the vehicle to the candidate track actually measured by the radar and the first distance.

[0161] The target trajectory determination module is used to determine the actual target trajectory detected by the radar based on the target distance differences corresponding to the M candidate trajectories and the least squares method.

[0162] In one implementation, the difference determination module is used to calculate the target distance difference corresponding to each candidate trajectory according to a first formula.

[0163] The first formula includes:

[0164]

[0165] Among them, zi This represents the target distance difference corresponding to the i-th candidate orbit. Let d represent the second distance corresponding to the i-th candidate trajectory, θ represent the target angle corresponding to the i-th candidate trajectory, and d represent the distance between the i-th and i-th candidate trajectories. i This represents the first distance corresponding to the i-th candidate track;

[0166] The difference determination module is used to calculate the first distance d according to the second formula. i The second formula includes:

[0167]

[0168] Where, x pv y pv Let x and y represent the x and y coordinates of the vehicle's position at the second time point, respectively. pr y pr d1 and d2 respectively represent the horizontal and vertical coordinates of the perpendicular point obtained on the i-th candidate track after drawing a perpendicular line from the vehicle's representative position at the second moment to the i-th candidate track in the high-precision map, and d0 represents the longitudinal installation distance between the radar and the IMU.

[0169] In one implementation, the target orbit determination module is used to determine the j-th candidate orbit as the target orbit according to a third formula.

[0170] The third formula includes:

[0171]

[0172] z j This represents the target distance difference corresponding to the j-th candidate orbit.

[0173] In one embodiment, the apparatus further includes:

[0174] The track detection unit is used to determine whether there is a track near the vehicle based on the vehicle's real-time positioning status and high-precision map.

[0175] The track detection unit includes:

[0176] The search module is used to find the vehicle's location in the real-time positioning status of the vehicle in the high-precision map;

[0177] The track detection module is used to determine the existence of a track near the vehicle when the distance between the vehicle's real-time positioning position and the track in the high-precision map is less than a second distance threshold, and the angle between the driving direction represented by the heading angle in the vehicle's real-time positioning state and the track in the high-precision map is within the target vertical range. The target vertical range includes 90 degrees.

[0178] In one embodiment, the apparatus further includes:

[0179] The region determination unit is used to determine the target orbit region before matching the first orbit features stored in the high-precision map with the second orbit features detected by radar in real time to determine the M candidate orbits within the target orbit region;

[0180] The area determination unit includes:

[0181] The lateral width determination module is used to determine the lateral width of the target track area by using the width of the current lane or the width after widening the current lane by the target distance.

[0182] The longitudinal length determination module is used to determine the longitudinal length of the target track area based on the vehicle's longitudinal positioning uncertainty and the radar ranging noise at the first moment.

[0183] The vehicle positioning device provided in this embodiment can, when determining the existence of a track near the vehicle based on the vehicle's real-time positioning status and a high-precision map, first match the first track features stored in the high-precision map with the second track features detected by the radar in real time to determine M candidate tracks within the target track area. Then, it uses a state estimation algorithm, the vehicle positioning status at the first moment, and the IMU information at the second moment to determine the vehicle positioning status at the second moment. Finally, it uses the state estimation algorithm and the relative position information of each of the M candidate tracks with respect to the vehicle in the high-precision map to correct the vehicle positioning status at the second moment, obtaining M corrected vehicle positioning states at the second moment, where the first moment is the moment immediately preceding the second moment. Then, based on the nearest neighbor matching algorithm, the M corrected vehicle positioning states at the second moment, and the relative position information of each candidate track with respect to the vehicle in the high-precision map, it determines the target track actually detected by the radar. Finally, it uses the corrected vehicle positioning state at the second moment calculated using the target track as the vehicle positioning state at the second moment. Therefore, this embodiment of the present disclosure can correct the vehicle's current positioning position based on the relative position information between the candidate track and the vehicle, the state estimation algorithm, and the nearest neighbor matching algorithm when the vehicle detects a track. This improves vehicle positioning accuracy in special scenarios such as ports while saving costs. Furthermore, when the vehicle passes the target track, the longitudinal coordinates of the vehicle's current positioning position are corrected based on the target track, the vehicle's current positioning position, and the real-time measured IMU information, further improving the positioning accuracy when the vehicle passes the target track.

[0184] Based on the above method embodiments, another embodiment of this disclosure provides a storage medium storing executable instructions thereon, which, when executed by a processor, cause the processor to implement the method described in any of the above method embodiments.

[0185] Based on the above method embodiments, another embodiment of this disclosure provides a vehicle, the vehicle including: one or more processors;

[0186] Storage device for storing one or more programs.

[0187] When the one or more programs are executed by the one or more processors, the vehicle performs the method as described in any of the above method embodiments.

[0188] The above-described system and device embodiments correspond to the method embodiments and have the same technical effects. For detailed descriptions, please refer to the method embodiments. The device embodiments are derived based on the method embodiments; detailed descriptions can be found in the method embodiments section, and will not be repeated here. Those skilled in the art will understand that the accompanying drawings are merely schematic diagrams of one embodiment, and the modules or processes shown in the drawings are not necessarily essential for implementing this disclosure.

[0189] Those skilled in the art will understand that the modules in the apparatus of the embodiments can be distributed in the apparatus of the embodiments as described in the embodiments, or they can be located in one or more devices different from this embodiment with corresponding changes. The modules of the above embodiments can be combined into one module, or they can be further divided into multiple sub-modules.

[0190] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of this disclosure, and are not intended to limit them. Although this disclosure has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of this disclosure.

Claims

1. A vehicle positioning method characterized by, The method comprises: In the case that the existence of a track near the vehicle is determined according to the real-time positioning state of the vehicle and the high-definition map, M candidate tracks in the target track area are determined by matching the first track feature stored in the high-definition map and the second track feature detected by the radar in real time, wherein M is a positive integer, and the vehicle is a smart car; The real-time positioning state of the vehicle at the second time is determined by using a state estimation algorithm, the real-time positioning state of the vehicle at the first time and the IMU information of the inertial navigation sensor at the second time, and the real-time positioning state of the vehicle at the second time is corrected respectively by using the state estimation algorithm and the relative position information of each candidate track in the M candidate tracks in the high-definition map and the vehicle, thereby obtaining M corrected real-time positioning states of the vehicle at the second time, wherein the first time is the previous time adjacent to the second time, and the second time is the time when the second track feature is detected by the radar; The target track actually detected by the radar is determined based on a nearest neighbor matching algorithm, the M corrected real-time positioning states of the vehicle at the second time, and the relative position information of each candidate track in the high-definition map and the vehicle, and the corrected real-time positioning state of the vehicle at the second time using the target track for calculation is taken as the real-time positioning state of the vehicle at the second time. The method further comprises: The vehicle positioning position in the real-time positioning state of the vehicle is searched in the high-definition map; In the case that the distance between the vehicle positioning position in the real-time positioning state of the vehicle and the track in the high-definition map is less than a second distance threshold, and the included angle between the driving direction represented by the heading angle in the real-time positioning state of the vehicle and the track in the high-definition map is located in a target perpendicular range, it is determined that the track exists near the vehicle, wherein the target perpendicular range includes 90 degrees.

2. The method of claim 1, wherein, The method further comprises: When the vehicle passes through the target track, the longitudinal coordinate of the vehicle positioning position at the current time is corrected according to the target track, the vehicle positioning position in the real-time positioning state of the vehicle at the current time and the real-time measured IMU information, wherein the current time is greater than or equal to the second time.

3. The method of claim 2, wherein, The method further comprises: When the vehicle passes through the target track, the longitudinal coordinate of the vehicle positioning position at the current time is corrected according to the target track, the vehicle positioning position in the real-time positioning state of the vehicle at the current time and the real-time measured IMU information, wherein the current time is greater than or equal to the second time. The method further comprises:

4. The method of claim 3, wherein, The reference position is determined when the vehicle passes through the target track based on the real-time measured IMU information, wherein the reference position is the intersection of the target track and the vehicle planning path in the high-definition map; In the case that the longitudinal error between the vehicle positioning position at the current time and the reference position is less than an error threshold, the longitudinal coordinate in the vehicle positioning position at the current time is corrected to the longitudinal coordinate of the reference position in the vehicle body coordinate system. The method further comprises: determine that the vehicle passes the target track in a case that a distance between a position of a vehicle positioning position at the current time in the high-definition map and the target track is less than a first distance threshold, and an error between a measured variance of the IMU information measured in the target time period and a reference measured variance is less than an error threshold.

5. The method of claim 4, wherein, In a case that the measured variance of the IMU information includes an accelerometer measured variance and a gyroscope measured variance, and the reference measured variance includes an accelerometer reference measured variance and a gyroscope reference measured variance, the error between the measured variance of the IMU information measured in the target time period and the reference measured variance being less than the error threshold includes that: in the target time period, the error between the accelerometer measured variance and the accelerometer reference measured variance is less than the error threshold, and the error between the gyroscope measured variance and the gyroscope reference measured variance is less than the error threshold.

6. The method of claim 1, wherein, The determining the target track actually detected by the radar based on the nearest neighbor matching algorithm, the M corrected second-time vehicle positioning states, and the relative position information of each candidate track in the high-definition map to the vehicle includes: determining a target distance difference corresponding to each candidate track according to the relative position information of each candidate track in the high-definition map to the vehicle, wherein the relative position information includes a first distance, a second distance, and a target included angle, the first distance is a vertical distance from a vehicle representative position at the second time to the candidate track, the vehicle representative position at the second time is a corresponding position of a vehicle positioning position in the high-definition map in the corrected second-time vehicle positioning state, the second distance is a longitudinal distance actually measured by the radar at the second time from the vehicle to the candidate track in a body coordinate system, the target included angle is an included angle between a driving direction represented by a heading angle in the corrected second-time vehicle positioning state and the candidate track in the high-definition map, and the target distance difference is a difference between the vertical distance actually measured by the radar from the vehicle to the candidate track and the first distance; determining the target track actually detected by the radar according to the target distance differences respectively corresponding to the M candidate tracks and a least square method.

7. The method of claim 6, wherein, The determining a target distance difference corresponding to each candidate track according to the relative position information of each candidate track in the high-definition map to the vehicle includes: calculating the target distance difference corresponding to each candidate track according to a first formula, the first formula includes: wherein the z i represents the target distance difference corresponding to the candidate orbit of the i th item, and the represents the second distance corresponding to the candidate orbit of the i th item, the θ represents the target angle corresponding to the candidate orbit of the i th item, and d i represents the first distance corresponding to the candidate orbit of the i th item; calculating the first distance d according to a second formula i , the second formula comprising: wherein the x pv , the y pv respectively represent the horizontal and vertical coordinates of the representative position of the vehicle at the second moment, the x pr , the y pr respectively represent the horizontal and vertical coordinates of the vertical point obtained on the i-th candidate track after the vertical line is drawn from the representative position of the vehicle at the second moment to the i-th candidate track in the high-definition map, and the d0 represents the installation longitudinal distance between the radar and the IMU.

8. The method of claim 6, wherein, the determining the target track actually detected by the radar according to the target distance differences respectively corresponding to the M candidate tracks and a least square method includes: determining that the jth candidate track is the target track according to a third formula, wherein the third formula includes: The z j represents the target distance difference corresponding to the candidate track described in the jth.

9. The method according to any one of claims 1-8, characterized in that, before determining the M candidate tracks in the target track region by matching a first track feature stored in the high-definition map and a second track feature detected by the radar in real time, the method further includes determining the target track region. The determining the target track region comprises: taking the width of the current lane or the width after the current lane is widened by the target distance as the lateral width of the target track region; determining the longitudinal length of the target track region according to the vehicle longitudinal positioning uncertainty at the first time and the ranging noise of the radar.

Images