Vehicle positioning method and system

Abstract

The present invention discloses a vehicle positioning method, comprising the following steps: Step 1: Collecting road image information and vehicle motion information; Step 2: Converting the collected road image information into a top view through an inverse perspective transformation method; Matching the road image information in the top view with a vector map to obtain the position of the top view in the vector map; Step 3: According to the position of the vehicle in the top view, the position of the vehicle in the vector map is obtained, thereby obtaining the position information of the vehicle; The present invention provides a vehicle positioning system. The present invention not only has low use cost, but also has good positioning effect, can not only quickly complete positioning, but also accurately position, which is particularly important for autonomous driving vehicles; Furthermore, the method or system provided by the present invention does not require high computer computing power.

Description

Technical Field

[0001] The present invention belongs to the field of autonomous driving, and in particular relates to a vehicle positioning method and system. Background Art

[0002] Vehicle positioning is a key technology in the field of autonomous driving. Currently, high-precision combined navigation, multi-line laser radar, Camera and other sensors are commonly used in combination with high-precision maps to achieve vehicle positioning, mainly using algorithms such as Kalman filtering and SLAM. The use of high-precision combined navigation has a high overall price and high cost of use; multi-line laser radar positioning not only has high computing power, but also has a high price and high requirements for computing platform resources. At present, expensive GPS and IMU devices are also used to achieve absolute positioning. However, this method has a large positioning deviation when the vehicle passes through tunnels and other places where the GPS signal and magnetic field environment are unstable for a long time. Summary of the invention

[0003] Purpose of the invention: In view of the problems existing in the prior art, the present invention proposes a vehicle positioning method based on low-cost sensors and with accurate positioning effect.

[0004] Technical solution: To achieve the above purpose, the present invention provides a vehicle positioning method, comprising the following steps:

[0005] Step 1: Collect road image information and vehicle motion information;

[0006] Step 2: Convert the collected road image information into a bird's-eye view by using an inverse perspective transformation method; match the road image information in the bird's-eye view with the vector map to obtain the position of the bird's-eye view in the vector map;

[0007] Step 3: According to the position of the vehicle in the top view, the position of the vehicle in the vector map is obtained, thereby obtaining the vehicle's position information;

[0008] Among them, step 3 includes horizontal positioning, vertical positioning and fusion positioning;

[0009] The lateral positioning obtains the lateral offset distance of the vehicle in the lane through the top view, and optimizes the vehicle motion information collected by the sensor, the lateral offset distance of the vehicle in the lane and the position information of the vehicle obtained by the fusion positioning through the first Kalman filter to obtain the state quantity output by the first Kalman filter; wherein the state quantity output by the first Kalman filter includes the lateral offset distance of the vehicle in the road, the lateral acceleration of the vehicle, the speed of the vehicle and the yaw angle of the vehicle;

[0010] The longitudinal positioning aligns the road surface markings in the top view with the road surface markings in the vector map to obtain the longitudinal displacement of the vehicle in the vector map; and optimizes the vehicle motion information collected by the sensor, the longitudinal displacement of the vehicle in the vector map and the position information of the vehicle obtained by the fusion positioning through the second Kalman filter to obtain the state quantity output by the second Kalman filter; the state quantity output by the second Kalman filter is the longitudinal displacement of the vehicle in the vector map, the running speed of the vehicle and the yaw angle of the vehicle;

[0011] The fusion positioning comprises the following steps:

[0012] Step 301: Obtain the position V of the vehicle in the vector map according to the output values ​​of the lateral positioning submodule and the longitudinal positioning submodule pos ;

[0013] Step 302: Take the pose V pos As the center, take multiple reference poses;

[0014] Step 303: Take out the vector map at the position V pos and a set of discrete lane points near multiple reference poses represents the set of discrete points of lane lines near the i-th pose in the vector map, i∈(1, 2, …, J+1);

[0015] Step 304: Take out the positions V in the top view respectively. pos and a set of discrete lane points near multiple reference poses Represents the set of discrete points of lane lines near the i-th pose in the top view

[0016] Step 305: Calculate the set for each pose and collection The minimum distance between them is taken, and the minimum corresponding posture of the minimum distance is taken as the posture of the current vehicle; the posture of the vehicle includes the horizontal coordinate, vertical coordinate and heading angle of the vehicle in the vector map.

[0017] Further, the method for obtaining the top view in step 2 is: according to the formula:

[0018]

[0019] The collected road image information is converted into a bird's-eye view; where x and y represent the horizontal and vertical coordinate values ​​in the converted bird's-eye view, respectively; h represents the height of the camera from the ground; β represents the elevation angle of the camera; α is the deflection angle of the pixel point in the road image collected by the camera relative to the camera focus; u and v represent the horizontal and vertical coordinates of the pixel in the road image collected by the camera; M' -1It is the inverse transformation of M', M = I*T, I is the intrinsic parameter matrix of the camera, and T is the extrinsic parameter matrix from the camera to the vehicle. The reconstructed M matrix is ​​more convenient for calculation.

[0020] Furthermore, the method for obtaining the elevation angle β of the camera is as follows: in the initial state, taking the measured elevation angle as a reference, selecting multiple elevation angles, performing top view conversion respectively, comparing the lane width in the converted top view with the lane width in the vector map, taking the difference between the lane width in the top view and the lane width in the vector map, and the elevation angle with the smallest difference is the elevation angle in the top view conversion calculation. The elevation angle β obtained in this way is more accurate, and thus the top view converted according to the elevation angle β is more accurate.

[0021] Furthermore, the method for selecting multiple pitch angles is: taking the pitch angle of the camera measured in the initial state as the reference pitch angle, taking 20 pitch angles before and after the reference pitch angle at the same angle interval; wherein the angle interval is 0.1°. This can effectively meet the calculation accuracy and speed up the calculation.

[0022] Furthermore, in the lateral positioning, an alarm signal is also issued according to the degree of deviation of the vehicle from the center line of the road; wherein the degree of deviation of the vehicle from the center line of the road is if Not less than Then an alarm signal is issued; d is the lateral offset distance of the vehicle in the lane, V w is the body width, R w is the lane width, s d This is a safe distance. This allows you to quickly know whether the vehicle is safe to drive.

[0023] Furthermore, the set and collection The method for obtaining the minimum distance between the two is as follows: select the set with fewer points as the reference set, traverse each point in the reference set and calculate the distance between each point in the set with more points, select the minimum distance value corresponding to each point in the reference set; obtain the minimum distance set of the reference set; add each value in the minimum distance set to obtain the set and collection This can complete the positioning faster and more accurately, and this method does not have high requirements on the hardware of the computer platform.

[0024] The present invention also provides a vehicle positioning system, including a data acquisition module, an image conversion and matching module and a vehicle positioning module; wherein,

[0025] The data acquisition module is used to collect vehicle operation data and road surface image information around the vehicle;

[0026] The image conversion and matching module converts the road image information around the vehicle collected by the data acquisition module into a top view through an inverse perspective transformation method; matches the road image information in the top view with the vector map to obtain the position of the top view in the vector map;

[0027] According to the position of the vehicle in the overhead view, the position of the vehicle in the vector map is obtained, thereby obtaining the positioning information of the vehicle.

[0028] Furthermore, the vehicle positioning module includes a lateral positioning submodule, a longitudinal positioning submodule and a fusion positioning submodule. The lateral positioning submodule and the longitudinal positioning submodule input the results to the fusion positioning submodule in real time. The fusion positioning submodule outputs the positioning results to the terminal in real time and sends them to the lateral positioning submodule and the longitudinal positioning submodule for optimization respectively; wherein:

[0029] The lateral positioning submodule obtains the lateral offset distance of the vehicle in the lane through the top view, and optimizes the vehicle motion information collected by the sensor, the lateral offset distance of the vehicle in the lane and the vehicle posture information obtained by the fusion positioning through the first Kalman filter to obtain the state quantity output by the first Kalman filter; wherein the state quantity output by the first Kalman filter includes the lateral offset distance of the vehicle in the road, the lateral acceleration of the vehicle, the speed of the vehicle and the yaw angle of the vehicle;

[0030] The longitudinal positioning submodule aligns the road markings in the top view with the road markings in the vector map to obtain the longitudinal displacement of the vehicle in the vector map; and optimizes the vehicle motion information collected by the sensor, the longitudinal displacement of the vehicle in the vector map and the fusion positioning to obtain the vehicle posture information to obtain the state quantity output by the second Kalman filter; the state quantity output by the second Kalman filter is the longitudinal displacement of the vehicle in the vector map, the running speed of the vehicle and the yaw angle of the vehicle;

[0031] The positioning method of the fusion positioning submodule is:

[0032] The vehicle’s position V in the vector map is obtained based on the output values ​​of the lateral positioning submodule and the longitudinal positioning submodule. pos ;

[0033] In posture V pos As the center, take multiple reference poses;

[0034] Take out the vector map at the pose V pos and a set of lane line discrete points near multiple reference poses represents the set of discrete points of lane lines near the i-th pose in the vector map, i∈(1, 2, …, J+1);

[0035] Take out the positions V in the top view respectively pos and a set of lane line discrete points near multiple reference poses represents the set of discrete points of lane lines near the i-th pose in the top view;

[0036] For each pose, the set is calculated separately and collection The minimum distance between them is taken, and the minimum corresponding posture of the minimum distance is taken as the posture of the current vehicle; the posture of the vehicle includes the horizontal coordinate, vertical coordinate and heading angle of the vehicle in the vector map.

[0037] Furthermore, the set and collection The method for obtaining the minimum distance between the two is as follows: select the set with fewer points as the reference set, traverse each point in the reference set and calculate the distance between each point in the set with more points, select the minimum distance value corresponding to each point in the reference set; obtain the minimum distance set with the reference set; add each value in the minimum distance set to obtain the set and collection The minimum distance between .

[0038] Furthermore, the data acquisition module includes a camera, which is arranged on the top of the vehicle, and its pitch angle and roll angle are both 0.

[0039] Beneficial effects: Compared with the prior art, the present invention not only has low cost, but also has good positioning effect. It can not only complete positioning quickly, but also accurately, which is especially important for autonomous driving vehicles. Furthermore, the method or system provided by the present invention does not require high computer computing power. BRIEF DESCRIPTION OF THE DRAWINGS

[0040] Figure 1 A schematic diagram of the structure of the system provided by the present invention;

[0041] Figure 2 A flow chart of the method provided by the present invention;

[0042] Figure 3 This is a schematic diagram of camera projection when the roll angle and yaw angle of the camera are 0;

[0043] Figure 4 This is a schematic diagram of a vehicle positioning module;

[0044] Figure 5 Schematic diagram of vehicle parameters optimized for the lateral positioning submodule;

[0045] Figure 6 Schematic diagram of vehicle parameters optimized for the longitudinal positioning submodule. DETAILED DESCRIPTION

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

[0047] Example 1: Figure 1 As shown, this embodiment provides a vehicle positioning system, which mainly includes a data acquisition module, an image conversion and matching module and a vehicle positioning module; wherein the data acquisition module mainly uses a camera and an inertial sensor (referred to as IMU) to collect relevant data of the vehicle; the camera is set on the top of the vehicle, and its pitch angle and roll angle are both 0; it is mainly used to collect road image information, including lane lines, turn arrows, stop lines, speed limit road signs, etc.; IMU is mainly used to collect vehicle motion information, including acceleration and rotation angle of vehicle motion, etc. The image conversion and matching module mainly converts the road image information collected by the data acquisition module into a top view through an inverse perspective transformation method, matches the road image information in the top view with the vector map, and obtains the position of the top view in the vector map; the vehicle positioning module obtains the position of the vehicle in the vector map according to the position of the vehicle in the top view, thereby obtaining the positioning information of the vehicle.

[0048] like Figure 2 As shown, this embodiment provides a positioning method based on a vehicle positioning system, which mainly includes the following steps:

[0049] Step 1: The data acquisition module collects road image information and vehicle motion information; the vehicle motion information includes the acceleration and rotation angle of the vehicle;

[0050] Step 2: The image conversion and matching module converts the road image information collected by the data acquisition module into a top view through the inverse perspective transformation method, and matches the road image information in the top view with the vector map to obtain the position of the top view in the vector map. Specifically, according to the formula: Complete the conversion, where h represents the height of the camera from the ground, and β represents the pitch angle of the camera. In the initial state, the pitch angle β is measured with a ruler or obtained by the external parameter calibration method.

[0051] The specific working principle is:

[0052] According to the camera imaging principle, the transformation from the space point in the vehicle coordinate system to the image coordinate system is as follows:

[0053]

[0054] Where u and v represent the horizontal and vertical coordinates of the pixels in the road image collected by the camera, and I is the internal parameter of the camera. Among them, c x Indicates the lateral offset of the camera optical axis in the image coordinate system, c y Indicates the horizontal and vertical offset of the camera optical axis in the image coordinate system, f x and f y Respectively represent the horizontal and vertical focal lengths. T is the external parameter from the camera to the vehicle. Among them, r represents the rotation vector as a 3*3 matrix, which is obtained by the pitch angle, roll angle, and heading angle. t is a 3*1 matrix, which is composed of the translation values ​​of x, y, and z respectively; x, y, and z represent the horizontal coordinate value, vertical coordinate value, and height coordinate value in the vehicle coordinate system respectively; that is, the horizontal coordinate value, vertical coordinate value, and height coordinate value in the top view. Zc is the actual depth value corresponding to the pixel point in the camera coordinate system. Among them, I is a 3*3 matrix, and T is a 3*4 matrix. Let M = I*T, then M = 3*4 matrix.

[0055]

[0056] Combining (1) we get:

[0057]

[0058] It can be seen from formula (2) that any point [x, y, z] in the three-dimensional space can be projected to [u, v] in the two-dimensional image. If [u, v] of the two-dimensional image is projected to the three-dimensional space [x, y, z], it does not hold because the M matrix cannot be inverted. However, if a certain latitude value of the three-dimensional space is assumed to be confirmed. The present invention can mainly solve the position of the pixel coordinate point on a known plane by reconstructing the M matrix and calculating Zc. The specific method is as follows:

[0059] (1) Assume that the height value of the projection plane z=h.

[0060]

[0061] Simplifying the above formula, we get:

[0062]

[0063] M' is a 3*3 matrix, that is, a square matrix, and its inverse transformation can be solved:

[0064]

[0065] like Figure 3As shown in the figure, o is the origin of the camera, oa is the projection height (assuming oa = h), h is the height of the camera from the ground, of is the focal length of the camera, gh is the image plane of the camera, d is the point on the ground, b is the corresponding point of d on the image, β is the pitch angle, then oc is the actual depth value corresponding to the pixel in the camera coordinate system. α is the deflection angle of point b relative to the camera focus.

[0066] According to the camera internal parameters, the coordinate value of b in the image, and the projection height of the camera, we can get:

[0067]

[0068] oc is equal to the z of the camera when the roll angle and yaw angle are 0 c Combining formula (10) and (6), we can get the (x, y) coordinates of the image space point [uv] in the z = h plane:

[0069]

[0070] From formula (11), it can be seen that the conversion relationship is mainly determined by the height h and the pitch angle β. The height value has a high accuracy through geometric measurement. However, the pitch angle measurement method of the camera is relatively difficult, and there will be a certain deviation due to the undulation of the road surface during the vehicle operation, which will cause the lane line detection to be unstable. At this time, the lane width in the transformed top view can be compared with the lane width in the vector map to obtain the optimal pitch angle. Taking the measured pitch angle as Pm as the reference, with the same angle interval, 20 pitch angles are taken before and after the reference pitch angle Pm; the angle interval is 0.1°. Each pitch angle is used as a parameter for inverse perspective transformation, and the transformed lane width is subtracted from the lane width in the vector map. The pitch angle corresponding to the minimum difference Dmin is taken as the standard pitch angle, and the inverse perspective transformation is performed to obtain the lane line, as well as road signals such as turn arrows, stop lines, speed limit road signs, and diamond signs to complete the conversion of the top view. Among them, the minimum difference Dmin needs to be less than the threshold Dt. If the minimum difference Dmin is not less than the threshold Dt, it means that there is a large deviation in the pitch angle and the calculation is wrong. Setting the threshold Dt can effectively ensure the accuracy and effectiveness of the calculation. In this embodiment, the threshold Dt is 10.

[0071] Step 3: The vehicle positioning module obtains the position of the vehicle in the vector map based on the position of the vehicle in the top view, thereby obtaining the vehicle's positioning information, such as Figure 4As shown, it specifically includes a lateral positioning submodule, a longitudinal positioning submodule and a fusion positioning submodule. The lateral positioning submodule and the longitudinal positioning submodule input the results to the fusion positioning submodule in real time. The fusion positioning submodule outputs the positioning results to the terminal in real time and sends them to the lateral positioning submodule and the longitudinal positioning submodule respectively; the lateral positioning submodule and the longitudinal positioning submodule both work based on the Kalman filter, and both work in the Frenet coordinate system.

[0072] The lateral positioning submodule specifically includes the following steps:

[0073] Step 311: Figure 5 As shown, the center axis of the lane in the top view is set as the reference center axis, and the distance between the center point of the vehicle and the reference center axis is calculated to obtain the lateral offset distance d of the vehicle in the lane.

[0074] Step 312: According to Calculate the degree of deviation of the vehicle body from the center line of the road. Less than It indicates that the vehicle is inside the lane, otherwise a warning message will be reported. w Indicates the width of the vehicle body, R w Indicates lane width, s d Indicates a safe distance.

[0075] Step 313: Input the vehicle motion information collected by the sensor, the lateral offset distance of the vehicle on the road, etc. into the first Kalman filter for optimization, and the state quantity X1 output by the first Kalman filter is the output of the lateral positioning submodule. The state quantity X1 output by the first Kalman filter is (d, a, v, θ), where d is the lateral offset distance of the vehicle on the road; a is the lateral acceleration of the vehicle; v is the running speed of the vehicle; θ is the angle between the central axis of the vehicle and the reference central axis, that is, the yaw angle of the vehicle.

[0076] The motion model is:

[0077]

[0078] Among them, d t is the distance from the center point of the vehicle to the reference center axis at time t; d t-1 v is the distance from the center point of the vehicle to the reference center axis at time t-1; t-1 is the running speed of the vehicle at time t-1; θ is the angle between the center axis of the vehicle and the reference center axis; a is the lateral acceleration of the vehicle; Δ t is the time difference between time t and time t-1. According to the formula: Get the lateral acceleration a of the vehicle; where a imuis the acceleration of the vehicle collected by the IMU; r is the road curvature radius marked in the vector map.

[0079] According to the motion model, the motion prediction model can be obtained:

[0080]

[0081] Among them, X 1t is the state value of each state in the state quantity X1 of the first Kalman filter at time t, X 1(t-1) is the state value of each state in the state quantity X1 of the first Kalman filter at time t-1, Q is the covariance of the prediction model, and is the motion noise of the prediction model;

[0082] The conversion relationship between the observed state and the state quantity is:

[0083]

[0084] In the above formula, Z1 is the observation state input to the first Kalman filter, Z1 = [d, a, v], the observation state can be the output data of the fusion positioning submodule, or the data collected by the sensor, and T represents the matrix transposition operation. R is the observation noise, which is set according to the observation state, that is, the observation noise used when the output data of the fusion positioning submodule and the data collected by the sensor are used as the observation state is different, and the observation noise is set according to the empirical value. According to the construction of the Kalman filter, a stable system state value can be obtained.

[0085] Longitudinal positioning submodule: align the road markings detected by vision with the road markings in the vector map to obtain the position of the vehicle in the vector map; design the Kalman filter based on this. Specifically, it includes the following steps:

[0086] Step 321: Figure 6 As shown, the visually detected road surface markings and the ground markings in the vector map are aligned to obtain the longitudinal displacement s of the vehicle in the vector map; that is, the road surface markings in the top view are matched according to the similarity with the road surface markings in the vector map, the area of ​​the vehicle in the vector map is found, and the longitudinal displacement s of the vehicle in the vector map is determined by the fixed road surface markings.

[0087] Step 322: The vehicle motion information collected by the sensor and the longitudinal displacement of the vehicle in the vector map obtained in step 321 are input into the second Kalman filter for optimization; the state quantity X2 output by the Kalman filter is the output of the longitudinal positioning submodule. The state quantity X2 output by the second Kalman filter is (s, v, θ), where s is the longitudinal displacement of the vehicle in the vector map; v is the running speed of the vehicle; θ is the angle between the central axis of the vehicle and the reference central axis, that is, the yaw angle of the vehicle.

[0088] The motion model is:

[0089] s t =s t-1 +v t11 *cosθ*Δ t ;

[0090] Among them, s t is the longitudinal displacement of the vehicle at time t, s t-1 is the longitudinal displacement of the vehicle at time t-1; v t-1 is the running speed of the vehicle at time t-1; θ is the angle between the center axis of the vehicle and the reference center axis; Δ t is the time difference between time t and time t-1.

[0091] According to the motion model, the motion prediction model can be obtained:

[0092]

[0093] Among them, X 2t is the state value of each state in the state quantity X2 of the second Kalman filter at time t, X 2(t-1) is the state value of each state in the state quantity X2 of the second Kalman filter at time t-1, Q is the covariance of the prediction model, and is the motion noise of the prediction model;

[0094] The conversion relationship between the observed state and the state quantity is:

[0095]

[0096] In the above formula, Z2 is the observation state input to the second Kalman filter, Z2 = [s, v, θ], the observation state can be the output data of the fusion positioning submodule, or the data collected by the sensor, and T represents the matrix transposition operation. R is the observation noise, which is set according to the observation state, that is, the observation noise used when the output data of the fusion positioning submodule and the data collected by the sensor are used as the observation state is different, and the observation noise is set according to the empirical value. According to the construction of the Kalman filter, a stable system state value can be obtained.

[0097] Fusion positioning submodule: fuse and match the output values ​​of the lateral positioning submodule and the longitudinal positioning submodule, the lane information detected by vision and the vector map to obtain the accurate positioning result of the vehicle in the vector map. The implementation steps are as follows:

[0098] Step 331: Obtain the vehicle's position information V in the vector map according to the output values ​​of the lateral positioning submodule and the longitudinal positioning submodule. posSince the top view has been matched with the vector map, the vehicle’s position information V in the vector map can be obtained by calculating the lateral offset distance d of the vehicle in the road output by the lateral positioning submodule, the longitudinal displacement s of the vehicle in the vector map output by the longitudinal positioning submodule, and the angle θ between the center axis of the vehicle and the reference center axis in the Frenet coordinate system. pos (x1, y1, yaw); x1 represents the horizontal coordinate of the vehicle in the vector map, y1 represents the vertical coordinate of the vehicle in the vector map, and yaw represents the heading angle of the vehicle, that is, the value in the frenet coordinate system is converted to the vector map coordinate system.

[0099] Step 332: Take pose V pos As the center, take J reference poses; in this embodiment, the pose V pos As the center, the vehicle's forward direction is the front, and the distance from the posture V pos 5 meters before and after, away from posture V pos Ten reference points were randomly set within a range of 2 meters.

[0100] Step 333: Take out the vector map at V pos and a set of discrete lane points near J reference poses represents the set of lane line discrete points near the i-th pose in the vector map, i∈(1, 2, …, J+1); in this embodiment, the vicinity represents the pose V pos Or 20m ahead of T reference positions.

[0101] Step 334: Take out the V pos and a set of discrete lane points near J reference poses represents the set of lane line discrete points near the i-th pose in the top view, i∈(1, 2, …, J+1); in this embodiment, the vicinity represents the pose V pos Or 20m ahead of 10 reference positions. According to the positional relationship between the vehicle and the lane in the top view, the coordinates of the corresponding lane in the top view in the vector map are obtained, so that The coordinates of each point in the vector map.

[0102] Step 335: Calculate the set for each pose separately and collection The minimum distance between them is taken, and the posture corresponding to the minimum distance is the posture of the current vehicle.

[0103] Among them, the calculation set and collection The method of minimizing the distance between is to select the set and The set with fewer points is the reference set. The distance between each point in the reference set and each point in the set with more points is calculated to obtain the minimum distance corresponding to each point in the reference set. Thus, the minimum distance set with the reference set is obtained. The sum of the minimum distances is the set corresponding to the pose. and collection The minimum distance between .

Claims

1. A vehicle positioning method, characterized in that: The following steps are involved: Step 1: Collect road image information and vehicle motion information; Step 2: Convert the collected road image information into a bird's-eye view by using an inverse perspective transformation method; match the road image information in the bird's-eye view with the vector map to obtain the position of the bird's-eye view in the vector map; Step 3: According to the position of the vehicle in the top view, the position of the vehicle in the vector map is obtained, thereby obtaining the position information of the vehicle: specifically including a lateral positioning submodule, a longitudinal positioning submodule and a fusion positioning submodule. The lateral positioning submodule and the longitudinal positioning submodule input the results to the fusion positioning submodule in real time. The fusion positioning submodule outputs the positioning results to the terminal in real time and sends them to the lateral positioning submodule and the longitudinal positioning submodule respectively; The lateral positioning submodule obtains the lateral offset distance of the vehicle in the lane through the top view, and optimizes the vehicle motion information collected by the sensor, the lateral offset distance of the vehicle in the lane and the posture information of the vehicle obtained by the fusion positioning through the first Kalman filter to obtain the state quantity output by the first Kalman filter; wherein the state quantity output by the first Kalman filter includes the lateral offset distance of the vehicle in the road, the lateral acceleration of the vehicle, the speed of the vehicle and the yaw angle of the vehicle; The longitudinal positioning submodule aligns the road surface markings in the top view with the road surface markings in the vector map to obtain the longitudinal displacement of the vehicle in the vector map; and optimizes the vehicle motion information collected by the sensor, the longitudinal displacement of the vehicle in the vector map and the position information of the vehicle obtained by fusion positioning through the second Kalman filter to obtain the state quantity output by the second Kalman filter; the state quantity output by the second Kalman filter is the longitudinal displacement of the vehicle in the vector map, the running speed of the vehicle and the yaw angle of the vehicle; The fusion positioning submodule comprises the following steps: Step 301: Obtain the position V of the vehicle in the vector map according to the output values ​​of the lateral positioning submodule and the longitudinal positioning submodule pos The lateral offset distance of the vehicle on the road output by the lateral positioning submodule, the longitudinal displacement of the vehicle in the vector map output by the longitudinal positioning submodule, and the angle between the central axis of the vehicle and the reference central axis are used to calculate the position information V of the vehicle in the vector map. pos ; Step 302: Take the pose V pos As the center, take multiple reference poses; Step 303: Take out the vector map at the position V pos and a set of discrete lane points near multiple reference poses represents the set of discrete lane line points near the i-th pose in the vector map, i∈(1, 2, ..., J+1); Step 304: Take out the positions V in the top view respectively. pos and a set of discrete lane points near multiple reference poses represents the set of discrete points of lane lines near the i-th pose in the top view; Step 305: Calculate the set for each pose and collection The minimum distance between them is taken, and the minimum corresponding posture of the minimum distance is taken as the posture of the current vehicle; the posture of the vehicle includes the horizontal coordinate, vertical coordinate and heading angle of the vehicle in the vector map.

2. The vehicle positioning method according to claim 1, characterized in that: The method for obtaining the top view in step 2 is: according to the formula: The collected road image information is converted into a bird's-eye view; where x and y represent the horizontal and vertical coordinate values ​​in the converted bird's-eye view, respectively; h represents the height of the camera from the ground; β represents the elevation angle of the camera; α is the deflection angle of the pixel point in the road image collected by the camera relative to the camera focus; u and v represent the horizontal and vertical coordinates of the pixel in the road image collected by the camera; M'- 1 It is the inverse transformation of M', M = I*T, I is the intrinsic parameter matrix of the camera, and T is the extrinsic parameter matrix from the camera to the vehicle.

3. The vehicle positioning method according to claim 2, characterized in that: The method for obtaining the elevation angle β of the camera is as follows: in the initial state, taking the measured elevation angle as a reference, selecting multiple elevation angles, performing overhead view conversion respectively, comparing the lane width in the converted overhead view with the lane width in the vector map, taking the difference between the lane width in the overhead view and the lane width in the vector map, and the elevation angle with the smallest difference is the elevation angle in the overhead view conversion calculation.

4. The vehicle positioning method according to claim 3, characterized in that: The method for selecting multiple elevation and pitch angles is: taking the elevation and pitch angle of the camera measured in the initial state as the reference elevation and pitch angle, and taking 20 elevation and pitch angles before and after the reference elevation and pitch angle at the same angle interval; The angle interval is 0.1°.

5. The vehicle positioning method according to claim 1, characterized in that: In lateral positioning, an alarm signal is also issued according to the degree of deviation of the vehicle from the center line of the road; the degree of deviation of the vehicle from the center line of the road is if Not less than Then an alarm signal is issued; d is the lateral offset distance of the vehicle in the lane, V w is the body width, R w is the lane width, s d For a safe distance.

6. The vehicle positioning method according to claim 1, characterized in that: The collection and collection The method for obtaining the minimum distance between the two is as follows: select the set with fewer points as the reference set, traverse each point in the reference set and calculate the distance between each point in the set with more points, select the minimum distance value corresponding to each point in the reference set; obtain the minimum distance set of the reference set; add each value in the minimum distance set to obtain the set and collection The minimum distance between .

7. A vehicle positioning system, characterized in that: It includes data acquisition module, image conversion and matching module and vehicle positioning module; among which, The data acquisition module is used to collect vehicle operation data and road surface image information around the vehicle; The image conversion and matching module converts the road image information around the vehicle collected by the data acquisition module into a top view through an inverse perspective transformation method; matches the road image information in the top view with the vector map to obtain the position of the top view in the vector map; According to the position of the vehicle in the top view, the position of the vehicle in the vector map is obtained, thereby obtaining the positioning information of the vehicle; The vehicle positioning module includes a lateral positioning submodule, a longitudinal positioning submodule and a fusion positioning submodule. The lateral positioning submodule and the longitudinal positioning submodule input the results to the fusion positioning submodule in real time. The fusion positioning submodule outputs the positioning results to the terminal in real time and sends them to the lateral positioning submodule and the longitudinal positioning submodule for optimization respectively; wherein: The lateral positioning submodule obtains the lateral offset distance of the vehicle in the lane through the top view, and optimizes the vehicle motion information collected by the sensor, the lateral offset distance of the vehicle in the lane and the vehicle posture information obtained by the fusion positioning through the first Kalman filter to obtain the state quantity output by the first Kalman filter; wherein the state quantity output by the first Kalman filter includes the lateral offset distance of the vehicle in the road, the lateral acceleration of the vehicle, the speed of the vehicle and the yaw angle of the vehicle; The longitudinal positioning submodule aligns the road markings in the top view with the road markings in the vector map to obtain the longitudinal displacement of the vehicle in the vector map; and optimizes the vehicle motion information collected by the sensor, the longitudinal displacement of the vehicle in the vector map and the fusion positioning to obtain the vehicle posture information to obtain the state quantity output by the second Kalman filter; the state quantity output by the second Kalman filter is the longitudinal displacement of the vehicle in the vector map, the running speed of the vehicle and the yaw angle of the vehicle; The positioning method of the fusion positioning submodule is: The vehicle’s position V in the vector map is obtained based on the output values ​​of the lateral positioning submodule and the longitudinal positioning submodule. pos ; In posture V pos As the center, take multiple reference poses; Take out the vector map at the pose V pos and a set of discrete lane points near multiple reference poses represents the set of discrete lane line points near the i-th pose in the vector map, i∈(1, 2, ..., J+1); Take out the positions V in the top view respectively pos and a set of discrete lane points near multiple reference poses represents the set of discrete points of lane lines near the i-th pose in the top view; For each pose, the set is calculated separately and collection The minimum distance between them is taken, and the minimum corresponding posture of the minimum distance is taken as the posture of the current vehicle; the posture of the vehicle includes the horizontal coordinate, vertical coordinate and heading angle of the vehicle in the vector map.

8. The vehicle positioning system according to claim 7, characterized in that: The collection and collection The method for obtaining the minimum distance between the two is as follows: select the set with fewer points as the reference set, traverse each point in the reference set and calculate the distance between each point in the set with more points, select the minimum distance value corresponding to each point in the reference set; obtain the minimum distance set with the reference set; add each value in the minimum distance set to obtain the set and collection The minimum distance between .

9. The vehicle positioning system according to claim 7, characterized in that: The data acquisition module includes a camera, which is arranged on the top of the vehicle, and its pitch angle and roll angle are both 0.

Images