A method of dead position correction based on millimeter wave vehicle-mounted radar scene recognition

Abstract

The present invention relates to a dead position correction method based on millimeter wave vehicle-mounted radar scene recognition, comprising the steps of: obtaining target point trace features detected by radar in a vehicle body coordinate system, and pre-processing the target point traces to obtain target point traces in an absolute coordinate system; clustering the obtained single-frame target point traces to form a rectangular frame; class matching the rectangular frames obtained in two adjacent frames, clustering the matched target point traces into one class to obtain a class rectangular frame; extracting features according to the class rectangular frame, obtaining a class rectangular frame that meets the characteristics of a railing or a curb, and processing the class rectangular frame to obtain a railing or a curb target; obtaining the angle difference between the identified railing or curb target and the reference railing or curb target, and compensating it to the next frame of the dead position information of the vehicle to obtain the next frame of the vehicle determined dead position information. The present invention directly obtains the heading angle deviation in real time and compensates it, thereby calibrating the heading angle deviation caused by the yaw angular velocity zero bias and improving the dead position calculation accuracy.

Description

Technical Field

[0001] The present invention relates to the technical field of dead reckoning, and in particular to a dead reckoning correction method based on millimeter-wave vehicle-mounted radar scene recognition. Background Art

[0002] When positioning a vehicle, GPS can provide 10Hz positioning information. However, the vehicle position also moves within the 0.1s interval between each GPS information. Especially when the vehicle is in an environment with weak GPS signals such as tunnels, there will be serious delays in vehicle positioning. Therefore, dead reckoning is an important means to ensure that the vehicle positioning system can still accurately position the vehicle in scenarios with no GPS signals or weak GPS signals.

[0003] Most of the current dead reckoning is to estimate the vehicle's heading angle, speed and other information with the help of other sensors after knowing the current position, and to estimate the position at the next moment. The heading angle is usually obtained by integrating the vehicle's yaw rate over time. The gyroscope is a sensor commonly used in vehicle positioning systems to estimate the yaw rate. It estimates the vehicle's yaw rate by measuring the angular rate of the object's three axes. However, due to the limitation of mechanical precision, there will be a small error in the angular rate. Therefore, the yaw rate error leads to the heading error, and the error accumulates more and more with time. Therefore, this error needs to be calibrated and corrected in the actual dead reckoning process. The current dead reckoning calibration module mainly counts the yaw rate zero bias, and this method can only calibrate the inherent deviation of the angular rate. It cannot calibrate the real-time error. For example, in the high-speed lane change scenario of the vehicle, the yaw rate is very small, and the sensitivity of the yaw rate sensor can no longer be identified. The vehicle's heading angle cannot be accurately estimated, affecting the dead reckoning.

[0004] Since the accuracy of dead reckoning depends on the accuracy of the yaw rate estimated by other sensors such as gyroscopes, its error is limited by the accuracy of mechanical manufacturing. The vehicle's posture changes during road driving, and the measurement error at different postures is unknown and cannot be compensated in real time. The heading angle is the time integral of the yaw rate, and the error is also the time integral. Therefore, the error of dead reckoning accumulates over time. Summary of the invention

[0005] In order to overcome the problem of certain errors in dead reckoning in the above-mentioned prior art, the present invention provides a dead position correction method based on millimeter wave vehicle-mounted radar scene recognition.

[0006] In order to solve the above technical problems, the technical solution of the present invention is as follows:

[0007] A method for dead-position correction based on millimeter-wave vehicle-mounted radar scene recognition comprises the following steps:

[0008] S10. Obtain the vehicle body information, and calculate the next frame of the vehicle's position information based on the vehicle body information; at the same time, obtain the target point trace features detected by the radar in the vehicle body coordinate system, and pre-process them to obtain the target point trace in the absolute coordinate system;

[0009] S20. Clustering the acquired single-frame target points, and forming a rectangular frame around the edges of the clustered target points;

[0010] S30. Perform class matching on the rectangular frame obtained in the previous frame and the rectangular frame obtained in the current frame, cluster the matched target points into one class, and update the rectangular frame of the current frame to obtain a class rectangular frame;

[0011] S40. Extract features according to the class rectangular frame, obtain the class rectangular frame that meets the characteristics of the railing or curb, and process it to obtain the railing or curb target;

[0012] S50. Obtain the angle difference between the identified guardrail or curb target and the reference guardrail or curb target, and compensate it into the next frame of the calculated positioning information of the vehicle to obtain the next frame of the vehicle's determined positioning information.

[0013] Further, as a preferred technical solution, step S10 specifically includes:

[0014] Obtain the vehicle body information, establish the body coordinates with the center of the vehicle as the origin, set the reference origin at the same time, and establish an absolute coordinate system with the reference origin as the coordinate origin;

[0015] According to the vehicle body information, obtain the vehicle's position information of the current frame in the absolute coordinate system, and calculate the vehicle's position information of the next frame in the absolute coordinate system based on the vehicle's position information of the current frame;

[0016] The target point trace features detected by the radar in the vehicle body coordinate system of the current frame are obtained, and the target point trace in the absolute coordinate system is obtained according to the navigation information of the vehicle in the current frame in the absolute coordinate system.

[0017] Further, as a preferred technical solution, the vehicle body information obtained includes vehicle position, vehicle speed and yaw rate;

[0018] The next frame of the vehicle's dead position information is calculated using the following formula:

[0019] θ=θ0+θ yaw *T;

[0020] X = X0 + v*T*cos(θ0);

[0021] Y = Y0 + v*T*sin(θ0);

[0022] Among them, [X0, Y0, θ0] is the vehicle's position information in the current frame, [X, Y, θ] is the vehicle's position information in the next frame, v is the vehicle speed, θ yaw is the yaw angular velocity, and T is the radar refresh time.

[0023] Furthermore, as a preferred technical solution, the target point trace in the absolute coordinate system is obtained by the following formula:

[0024] X1-x*cos(O0)+y*sin(O0)+X0;

[0025] Y1-x*cos(O0+90)+y*cos(O0)+Y0;

[0026] Among them, [X0.Y0, θ0] is the positioning information of the vehicle in the current frame, [x, y] is the target point track feature detected by the radar in the vehicle body coordinate system of the current frame, and [X1, Y1] is the target point track coordinate in the absolute coordinate system.

[0027] Further, as a preferred technical solution, step S20 specifically includes:

[0028] The DBSCAN algorithm is used to cluster the acquired single-frame target points according to the dynamic wave gate of the guardrail and curb clustering, and the edges of the clustered target point sets are formed into a rectangular frame.

[0029] Furthermore, as a preferred technical solution, the single-frame target points are clustered using the following formula:

[0030] deltX1=|delt x1*cos(θ)|+|delt y1*sin(θ))|;

[0031] deltY1=|delt x1*cos(θ+90)|+|d delty1*cos(θ))|;

[0032] Among them, [x1, X1][y1, Y1] represents the rectangular frame formed by the edge of the clustered target point trace set, delt x1, delt y1, represent the set guardrail and curb clustering dynamic wave gate, delt x1 = 1, delt y1 = 3.

[0033] Further, as a preferred technical solution, in step S30, the matched target point traces meet the following conditions:

[0034] x_cur1∈[x1-delt x1, X1+delt x1] or X_cur1∈[x1-delt x1, X1+delt x1];

[0035] at the same time, Or Y_cur1∈|y1-delt y1,Y1+delt y1;

[0036] Among them, delt x1, delt y1, represent the dynamic wave gate of guardrail and curb clustering, [x1, X1] [y1, Y1] represent the rectangular box of the previous frame, and [x_cur1, X_cur1] [y_cur1, Y_cur1] represent the rectangular box of the current frame.

[0037] Further, as a preferred technical solution, step S40 specifically includes:

[0038] Obtaining the number of class points and the class length from the class rectangular frame, and identifying the class rectangular frame whose number of class points meets a first preset threshold and whose class length meets a second preset threshold as an edge feature of a railing or a curb;

[0039] The rectangular frame identified as the edge feature of the railing or curb is fitted to obtain the characteristic parameters of the railing or curb target.

[0040] Further, as a preferred technical solution, the characteristic parameter of the railing or curb target obtained is the straight line where the railing or curb target is located in the absolute coordinate system.

[0041] Further, as a preferred technical solution, the angle difference between the railing or curb target identified in step S50 and the reference railing or curb target is the angle between the straight line where the railing or curb target is located in the absolute coordinate system and the straight line where the reference railing or curb target is located in the absolute coordinate system.

[0042] Compared with the prior art, the technical solution of the present invention has the following beneficial effects:

[0043] The present invention realizes the correction of vehicle position through scene information recognized by radar, directly obtains the heading angle deviation in real time and compensates it. Compared with the existing technology of calibrating the yaw angular velocity zero bias to achieve position correction, it can not only calibrate the heading angle deviation caused by the yaw angular velocity zero bias, but also calibrate the heading angle deviation caused by the yaw angular velocity accuracy error, and the calibration difficulty is reduced. It is also decoupled from time and does not accumulate with time, thereby improving the dead reckoning accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0044] Figure 1 It is a flowchart of the present invention.

[0045] Figure 2 This is a schematic diagram of inter-frame class matching of the present invention.

[0046] Figure 3 This is a schematic diagram of feature extraction of the present invention.

[0047] Figure 4Schematic diagram of heading angle compensation of the present invention.

[0048] The drawings are only used for illustrative purposes and should not be construed as limitations on this patent. In order to better illustrate this embodiment, some parts of the drawings may be omitted, enlarged or reduced, and do not represent the size of the actual product. For those skilled in the art, it is understandable that some well-known structures and their descriptions in the drawings may be omitted. The same or similar numbers correspond to the same or similar parts. The terms describing the positional relationship in the drawings are only used for illustrative purposes and should not be construed as limitations on this patent. DETAILED DESCRIPTION

[0049] The preferred embodiments of the present invention are described in detail below in conjunction with the accompanying drawings, so that the advantages and features of the present invention can be more easily understood by those skilled in the art, thereby making a clearer definition of the protection scope of the present invention.

[0050] The same or similar numbers in the drawings of the embodiments of the present invention correspond to the same or similar parts; in the description of the present invention, it should be understood that if the terms "upper", "lower", "left", "right", "top", "bottom", "inside", "outside" and the like indicate orientations or positional relationships based on the orientations or positional relationships shown in the drawings, they are only for the convenience of describing the present invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation. Therefore, the terms describing the positional relationship in the drawings are only used for illustrative purposes and cannot be understood as limitations on this patent.

[0051] In addition, if there are terms such as "first" and "second", they are only used for descriptive purposes, mainly to distinguish different devices, elements or components (the specific types and structures may be the same or different), and are not used to indicate or imply the relative importance and quantity of the indicated devices, elements or components, and cannot be understood as indicating or implying relative importance.

[0052] Example 1

[0053] This embodiment discloses a method for dead-position correction based on millimeter-wave vehicle-mounted radar scene recognition, such as Figure 1 As shown, the steps include:

[0054] S10. Obtain the vehicle body information, and calculate the next frame of the vehicle's position information based on the vehicle body information; at the same time, obtain the target point trace features detected by the radar in the vehicle body coordinate system, and pre-process them to obtain the target point trace in the absolute coordinate system.

[0055] This step specifically includes:

[0056] S101. Obtain the vehicle body information, establish the body coordinates with the center of the vehicle as the origin, set the reference origin at the same time, and establish an absolute coordinate system with the reference origin as the coordinate origin.

[0057] In this step, the vehicle body information obtained includes the vehicle position [X0, Y1, θ0], vehicle speed v and yaw angular velocity θ yaw , whose vehicle position includes vehicle coordinates [X0, Y0] and heading angle θ0.

[0058] Assume that the vehicle body information obtained in the current frame is the initial information, that is, [X0, Y0, θ 0 ]=[0,0,0], then the vehicle’s position information in the current frame in the vehicle body coordinate system and the absolute coordinate system is [X0,Y0,θ0].

[0059] S102. According to the vehicle body information, obtain the vehicle's position information of the current frame in the absolute coordinate system, and calculate the vehicle's position information of the next frame in the absolute coordinate system according to the vehicle's position information of the current frame.

[0060] Referring to step S101, assuming that the vehicle's position information in the current frame in the absolute coordinate system is [X0, Y0, θ0],

[0061] The next frame of the vehicle's dead position information is calculated using the following formula:

[0062] θ=θ0+θ yaw *T;

[0063] X = X0 + v*T*cos(θ0);

[0064] Y = Y0 + v*T*sin(θ0);

[0065] Among them, [X0, Y0, θ0] is the vehicle's position information in the current frame, [X, Y, θ] is the vehicle's position information in the next frame, v is the vehicle speed, θ yaw is the yaw angular velocity, and T is the radar refresh time.

[0066] S103. Obtain target point trace features detected by the radar in the vehicle body coordinate system of the current frame, and obtain the target point trace in the absolute coordinate system according to the navigation information of the vehicle in the current frame in the absolute coordinate system.

[0067] In this step, the target point trajectory in the absolute coordinate system is obtained by the following formula:

[0068] X1=x*cos(θ0)+y*sin(θ0)+X0;

[0069] Y1=x*cos(θ0+90)+y*cos(θ0)+Y0;

[0070] Among them, [X0, Y0, θ0] is the positioning information of the vehicle in the current frame, [x, y] is the target point track feature detected by the radar in the vehicle body coordinate system of the current frame, and [X1, Y1] is the target point track coordinate in the absolute coordinate system.

[0071] S20. Cluster the acquired single-frame target traces, and form a rectangular frame around the edges of the clustered target trace set.

[0072] This step is specifically as follows: using the DBSCAN algorithm, clustering the acquired single-frame target traces according to the dynamic wave gate of the guardrail and curb clustering, and forming a rectangular frame around the edge of the clustered target trace set.

[0073] For example, in this step, the guardrail or curb clustering dynamic wave gates are set to delt x1, delt y1, where delt x1 = 1, delt y1 = 3;

[0074] The single-frame target points are clustered using the following formula:

[0075] deltX1=|delt x1*cos((θ)|+|delt y1*sin(θ))|;

[0076] deltY1=|delt x1*cos(θ+90)|+|d delt y1*cos(θ))|;

[0077] The rectangular frame formed by the edge of the clustered target point set is recorded as X1, Y1, and its range is [x1, X1][y1, Y1].

[0078] S30. Perform class matching between the rectangular frame obtained in the previous frame and the rectangular frame obtained in the current frame, cluster the matched target points into one class, and update the rectangular frame of the current frame to obtain a class rectangular frame.

[0079] In this step, assuming that the rectangular box obtained in the previous frame is [x1, X1] [y1, Y1], and the rectangular box obtained in the current frame is [x_cur1, X_cur1][y_cur1Y_cur1], [x1, X1] [y1, Y1] is matched with [x_cur1, X_cur1][y_cur1, Y_cur1], and the target points that meet the following conditions are considered to be matched and clustered into one category:

[0080] x_cur1∈[x1-delt x1, X1+delt x1] or X_cur1∈[x1-delt x1, X1+delt x1];

[0081] At the same time, y_cur1∈[y1-delt y1, Y1+delt y1] or Y_cur1∈[y1-delt y1, Y1+delt y1;

[0082] Among them, delt x1, delt y1, represent the dynamic wave gates of guardrail and curb clustering.

[0083] like Figure 2 As shown: the target point traces in the previous frame rectangle box that match the current frame rectangle box are updated to the current frame rectangle box, and the current frame rectangle box parameters are updated synchronously to obtain a quasi-rectangular box.

[0084] S40. Extract features based on the class rectangular frame, obtain the class rectangular frame that meets the characteristics of the railing or curb, and process it to obtain the railing or curb target.

[0085] This step specifically includes:

[0086] S401. Obtain the number of class points and the class length from the class rectangular frame, and identify the class rectangular frame whose number of class points meets a first preset threshold and whose class length meets a second preset threshold as an edge feature of a guardrail or a curb.

[0087] In this step, assuming that the class rectangle is [x1, X1][y1, Y1], and the number of class points is N, the class length L is obtained by the following formula:

[0088] A class rectangular box whose number of class points N meets a first preset threshold and whose class length L meets a second preset threshold is identified as an edge feature of a railing or a curb.

[0089] In this embodiment, the first preset threshold is set in the range of 8-12, and the second preset threshold is set in the range of 4-6m. As a preferred embodiment, the first preset threshold is set to 10, and the second preset threshold is set to 5m. Therefore, the rectangular frame satisfying N>10 and L>5m is identified as the edge feature of the railing or curb.

[0090] S402. Fitting a quasi-rectangular frame identified as an edge feature of a railing or a curb to obtain characteristic parameters of the railing or curb target.

[0091] In this step, the characteristic parameter of the railing or curb target obtained is the straight line where the railing or curb target is located in the absolute coordinate system.

[0092] like Figure 3 As shown, a straight line fitting equation is used to perform straight line fitting on the rectangular frame identified as the edge feature of the guardrail or curb, as follows:

[0093] X=kY+b

[0094] Among them, X represents the straight line where the railing or curb target is located in the absolute coordinate system, k represents the slope of the current railing, and b represents the intercept of the railing on the X-axis.

[0095] S50. Obtain the angle difference between the identified guardrail or curb target and the reference guardrail or curb target, and compensate it into the next frame of the calculated positioning information of the vehicle to obtain the next frame of the vehicle's determined positioning information.

[0096] The angle difference between the railing or curb target identified in this step and the reference railing or curb target is the angle between the straight line where the railing or curb target is located in the absolute coordinate system and the straight line where the reference railing or curb target is located in the absolute coordinate system.

[0097] That is, the angle difference between the identified guardrail target and the reference guardrail target is the angle between the straight line where the identified guardrail target is located in the absolute coordinate system and the straight line where the reference guardrail target is located in the absolute coordinate system; or, the angle difference between the identified curb target and the reference curb target is the angle between the straight line where the identified curb target is located in the absolute coordinate system and the straight line where the reference curb target is located in the absolute coordinate system.

[0098] like Figure 4 As shown, assuming that the angle difference between the identified guardrail or curb target and the reference guardrail or curb target is α, the positioning information of the vehicle in the next frame calculated in step S101 is [X, Y, θ], that is, the calculated heading angle of the vehicle in the next frame is θ, then the heading angle of the vehicle is compensated, that is, the heading angle determined by the vehicle in the next frame is θ1=θ+α, therefore, the positioning information determined by the vehicle in the next frame is [X, Y, θ1].

[0099] Obviously, the above embodiments of the present invention are merely examples for clearly illustrating the present invention, and are not intended to limit the embodiments of the present invention. For those skilled in the art, other different forms of changes or modifications can be made based on the above description. It is not necessary and impossible to list all the embodiments here. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the protection scope of the claims of the present invention.

Claims

1. A method for dead-position correction based on millimeter-wave vehicle-mounted radar scene recognition, characterized in that: Includes steps: S10. Obtain the vehicle body information, and calculate the next frame of the vehicle's position information based on the vehicle body information; at the same time, obtain the target point trace features detected by the radar in the vehicle body coordinate system, and pre-process them to obtain the target point trace in the absolute coordinate system; S20. Clustering the acquired single-frame target points, and forming a rectangular frame around the edges of the clustered target points; S30. Perform class matching on the rectangular frame obtained in the previous frame and the rectangular frame obtained in the current frame, cluster the matched target points into one class, and update the rectangular frame of the current frame to obtain a class rectangular frame; S40. Extract features according to the class rectangular frame, obtain the class rectangular frame that meets the characteristics of the railing or curb, and process it to obtain the railing or curb target; S50. Obtain the angle difference between the identified guardrail or roadside target and the reference guardrail or roadside target, and compensate it to the next frame of the vehicle's position information calculated to obtain the next frame of the vehicle's position information; Wherein, step S40 specifically includes: Obtaining the number of class points and the class length from the class rectangular frame, and identifying the class rectangular frame whose number of class points meets a first preset threshold and whose class length meets a second preset threshold as an edge feature of a railing or a curb; Fitting a rectangular frame identified as an edge feature of a railing or a curb to obtain characteristic parameters of the railing or curb target; Step S20 specifically includes: The DBSCAN algorithm is used to cluster the acquired single-frame target points according to the dynamic wave gate of the guardrail and curb clustering, and the edges of the clustered target point sets are formed into a rectangular frame; The single-frame target points are clustered using the following formula: deltX1=|deltx1*cos(θ)|+|delty1*sin(θ)|; deltY1=|deltx1*cos(θ+90°)|+|delty1*cos(θ)|; Among them, [x1, X1][y1, Y1] represents the rectangular frame formed by the edge of the clustered target point set, deltx1, delty1, represent the set guardrail and curb clustering dynamic wave gate, deltx1=1, delty1=3.

2. The method for dead-position correction based on millimeter-wave vehicle-mounted radar scene recognition according to claim 1 is characterized in that: Step S10 specifically includes: Obtain the vehicle body information, establish the vehicle body coordinate system with the center of the vehicle as the origin, set the reference origin at the same time, and establish the absolute coordinate system with the reference origin as the coordinate origin; According to the vehicle body information, obtain the vehicle's position information of the current frame in the absolute coordinate system, and calculate the vehicle's position information of the next frame in the absolute coordinate system based on the vehicle's position information of the current frame; The target point trace features detected by the radar in the current frame vehicle body coordinate system are obtained, and the target point trace in the absolute coordinate system is obtained according to the navigation information of the vehicle in the current frame in the absolute coordinate system.

3. The method for dead position correction based on millimeter wave vehicle-mounted radar scene recognition according to claim 2 is characterized in that: The vehicle body information obtained includes vehicle position, vehicle speed and yaw rate; The next frame of the vehicle's dead position information is calculated using the following formula: θ=θ0+θ yaw *T; X = X0 + v*T*cos(θ0); Y = Y0 + v*T*sin(θ0); Among them, [X0, Y0, θ0] is the position information of the vehicle in the current frame, [X, Y, θ] is the position information of the vehicle in the next frame, v is the vehicle speed, θ yaw is the yaw angular velocity, and T is the radar refresh time.

4. The method for dead position correction based on millimeter wave vehicle-mounted radar scene recognition according to claim 2 is characterized in that: The target point trace in the absolute coordinate system is obtained by the following formula: X1=x*cos(θ0)+y*sin(θ0)+X0; Y1=x*cos(θ0+90°)+y*cos(θ0)+Y0; Among them, [X0, Y0, θ0] is the positioning information of the vehicle in the current frame, [x, y] is the target point track feature detected by the radar in the vehicle body coordinate system of the current frame, and [X1, Y1] is the target point track coordinate in the absolute coordinate system.

5. The method for dead position correction based on millimeter wave vehicle-mounted radar scene recognition according to claim 1, characterized in that: In step S30, the matched target point traces meet the following conditions: x_cur1∈[x1-deltx1,X1+deltx1] or X_cur1∈[x1-deltx1,X1+deltx1]; At the same time, y_cur1∈[y1-delty1, Y1+delty1] or Y_cur1∈[y1-delty1, Y1+delty1]; Among them, delt x1, delt y1, represent the dynamic wave gate of guardrail and curb clustering, [x1, X1][y1, Y1] represent the rectangular box formed by the edge of the target point trace set after clustering in the previous frame, and [x_cur1, X_cur1][y_cur1, Y_cur1] represent the rectangular box formed by the edge of the target point trace set after clustering in the current frame.

6. The method for dead position correction based on millimeter wave vehicle-mounted radar scene recognition according to claim 1, characterized in that: The acquired characteristic parameter of the guardrail or curb target is the straight line where the guardrail or curb target is located in the absolute coordinate system.

7. The method for dead position correction based on millimeter wave vehicle-mounted radar scene recognition according to claim 6, characterized in that: The angle difference between the railing or curb target identified in step S50 and the reference railing or curb target is the angle between the straight line where the railing or curb target is located in the absolute coordinate system and the straight line where the reference railing or curb target is located in the absolute coordinate system.

Images