CN114375406B - Target speed estimation method, device and storage medium - Google Patents
Target speed estimation method, device and storage medium Download PDFInfo
- Publication number
- CN114375406B CN114375406B CN202180005033.3A CN202180005033A CN114375406B CN 114375406 B CN114375406 B CN 114375406B CN 202180005033 A CN202180005033 A CN 202180005033A CN 114375406 B CN114375406 B CN 114375406B
- Authority
- CN
- China
- Prior art keywords
- target
- speed
- boundary
- point
- vehicle
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
- G01S17/50—Systems of measurement based on relative movement of target
- G01S17/58—Velocity or trajectory determination systems; Sense-of-movement determination systems
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
- G01S13/02—Systems using reflection of radio waves, e.g. primary radar systems; Analogous systems
- G01S13/50—Systems of measurement based on relative movement of target
- G01S13/58—Velocity or trajectory determination systems; Sense-of-movement determination systems
- G01S13/588—Velocity or trajectory determination systems; Sense-of-movement determination systems deriving the velocity value from the range measurement
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Electromagnetism (AREA)
- Computer Networks & Wireless Communication (AREA)
- General Physics & Mathematics (AREA)
- Optical Radar Systems And Details Thereof (AREA)
- Traffic Control Systems (AREA)
Abstract
The invention discloses a target speed estimation method, a target speed estimation device, a target speed estimation control method, a target speed estimation control device, a target speed estimation control method, a target speed estimation control device and a vehicle. The target speed estimation method comprises the steps of calculating a first candidate speed and a second candidate speed according to a first speed calculation mode and a second speed calculation mode respectively, determining the first candidate speed obtained according to the first speed calculation mode as the speed of a target when the target is located in an area outside a first boundary taking a vehicle as a symmetrical center, and determining the second candidate speed obtained according to the second speed calculation mode as the speed of the target on the contrary. According to the invention, two speed estimation modes are adopted to carry out speed estimation on the obstacle target, one speed estimation result is selected as the final estimated speed of the target according to the different positions of the obstacle target relative to the vehicle, and the accuracy of target speed estimation is improved.
Description
The present application claims priority from chinese patent application entitled "target speed estimation method, apparatus, and storage medium" filed on month 4 of 2021, 12, application number 2021103895278, the disclosure of which is incorporated herein by reference.
Technical Field
The present invention relates to the field of unmanned technology, and in particular to a target speed estimation method, apparatus, control device, computer readable storage medium, computer program product containing instructions, and vehicle.
Background
In the unmanned technique, two main types of obstacle target speed acquisition modes exist at present. The first is obtained by direct measurement with millimeter wave radar. The millimeter wave radar can accurately obtain the relative speed of the target, but the conventional millimeter wave radar configuration has a certain observation blind area (usually within a relatively short distance range from the left to the right of the vehicle), so that the automatic driving vehicle cannot obtain the speed information of the target in the blind area. The second method is to calculate the speed information of the target through the position change of the obstacle target detected by the laser radar, and the method has small observation blind area, but the calculated speed of the target has larger influence on the measuring position error of the laser radar.
Disclosure of Invention
The object of the present invention is to provide a target speed estimation method, a device, a control apparatus, a computer-readable storage medium, a computer program product containing instructions, and a vehicle, which perform speed estimation on an obstacle target in two different speed estimation manners, and select a speed estimation result as a final estimated speed of the target according to the difference of the position of the obstacle target relative to the vehicle, thereby improving the accuracy of target speed estimation.
In a first aspect of the present invention, a target speed estimation method is provided, including the steps of:
Calculating a first to-be-selected speed and a second to-be-selected speed according to the first speed calculation mode and the second speed calculation mode respectively;
When the target is determined to be positioned in an area outside a first boundary taking the vehicle as a symmetry center, determining a first speed to be selected obtained according to a first speed calculation mode as the speed of the target;
when the target is determined to be positioned in the area which is positioned in the first boundary with the vehicle as the symmetry center, determining a second candidate speed obtained according to the second speed calculation mode as the speed of the target;
Wherein, calculate the first speed of choosing according to the first speed calculation mode, include:
When the target is determined to be positioned in the area which is positioned in the second boundary with the own vehicle as the symmetry center, the closest point of the target detected by the laser radar is used as a target tracking point;
estimating a first speed to be selected based on the target tracking point;
the second candidate speed is calculated according to a second speed calculation mode, and the method comprises the following steps:
Calculating the moving speed of each target frame corner point and each target tracking point by utilizing the position data of the target frame corner point and the target tracking point obtained by the laser radar;
And estimating a second candidate speed according to the moving speed of each target frame corner point and each target tracking point.
In a second aspect of the present invention, there is provided a target speed estimating apparatus comprising:
the first speed calculating unit is used for calculating a first speed to be selected according to a first speed calculating mode;
a second speed calculating unit for calculating a second candidate speed according to the second speed calculating mode
A speed determining unit, configured to determine, when the target is located in an area outside a first boundary with the own vehicle as a center of symmetry, a first speed to be selected as a speed of the target, and conversely determine a second speed to be selected as a speed of the target;
wherein the first speed calculation unit includes:
The tracking point determining module is used for determining that when the target is positioned in an area outside a second boundary taking the vehicle as a symmetrical center, the closest point of the target detected by the laser radar is taken as a target tracking point;
the first speed estimation module is used for estimating a first speed to be selected based on the target tracking point;
Wherein the second speed calculation unit includes:
the target point speed calculation module is used for calculating the moving speed of each target frame angular point and each target tracking point by utilizing the position data of the target frame angular point and the target tracking point obtained by the laser radar;
And the second speed estimation module is used for estimating a second candidate speed according to the moving speed of each target frame corner point and each target tracking point.
In a third aspect of the present invention, a control device is provided, comprising at least one processor, and a memory communicatively coupled to the at least one processor, wherein the memory stores instructions executable by the at least one processor, the instructions being executable by the at least one processor to enable the at least one processor to perform the target speed estimation method according to the first aspect of the present invention.
In a fourth aspect of the present invention, there is provided a computer readable storage medium comprising a program or instructions which, when run on a computer, implement the target speed estimation method according to the first aspect of the present invention.
In a fifth aspect of the invention, a computer program product is provided comprising instructions which, when run on a computer, cause the computer to perform the target speed estimation method according to the first aspect of the invention.
In a sixth aspect of the present invention, there is provided a vehicle including the control apparatus according to the third aspect of the present invention.
According to the invention, two different speed estimation modes are adopted to estimate the speed of the obstacle target respectively, and then one speed estimation result is selected as the final estimated speed of the target according to the different positions of the obstacle target relative to the vehicle, so that the problem that the automatic driving vehicle cannot acquire the speed information of the target in the millimeter wave radar blind area is solved, the problem that the laser radar measurement result is greatly influenced by the position is solved, and the accuracy of the speed estimation of the target is improved.
Drawings
FIG. 1 is a flow chart of a target speed estimation method according to an embodiment of the invention;
FIG. 2 is a schematic view of a closed first boundary according to an embodiment of the present invention;
FIG. 3 is a flowchart illustrating a first alternate speed calculation according to an embodiment of the present invention;
FIG. 4 is a flow chart showing the calculation of a second candidate speed according to an embodiment of the present invention
FIG. 5 is a schematic diagram of the arrangement of the first boundary according to an embodiment of the present invention;
FIG. 6 is a schematic diagram of tracking point determination and switching by a second boundary when calculating a first speed to be selected according to an embodiment of the present invention;
FIG. 7 is a schematic diagram of velocity calculation of a uniform linear motion model according to an embodiment of the present invention;
FIG. 8 is a schematic diagram of a target corner and a target center point of a lidar for calculating a second candidate speed according to the embodiment of the present invention;
FIG. 9 is a schematic diagram of calculating displacement speeds of a target angular point and a target center point of a lidar and determining a target speed according to an embodiment of the present invention;
Fig. 10 is a schematic block diagram of a target speed estimation device according to an embodiment of the present invention.
Detailed Description
The invention is described in further detail below with reference to the drawings and the specific examples. It should be understood that the specific embodiments described herein are for purposes of illustration only and are not intended to limit the scope of the invention.
It should be noted that:
the term "own vehicle" in the present invention refers to a vehicle for performing the target speed estimation method proposed by the embodiment of the present invention, that is, a current vehicle mounted with sensor devices such as a laser radar, a millimeter wave radar, and the like.
The term "target speed" as used herein refers to a value of the speed of a target (obstacle) relative to a vehicle in a vehicle coordinate system, and may be referred to as a relative speed of the target.
At present, a laser radar commonly used in the market does not have penetrability, for a large target such as a vehicle, only a point cloud facing the side of the vehicle can be obtained, the point cloud facing away from the side of the vehicle cannot be obtained, in addition, the influence of self-shielding of the target or mutual shielding among objects can be possibly caused, the point cloud obtained by the laser radar cannot cover the whole target more completely, therefore, a target detection module of the laser radar is usually required to complement the target point cloud in a deep learning mode on the basis of the obtained original point cloud, then target frame regression is carried out based on the point cloud data after the complement processing, and information such as the length, the width and the height of the target, the central point position of the target and the like is output. However, when the target is farther away from the vehicle, the original point cloud data acquired by the laser radar is smaller, the error between the output result and the real situation is larger, otherwise, when the target is closer to the vehicle, the original point cloud data acquired by the laser radar is more, and the output result is more accurate.
At present, millimeter wave radar configuration commonly used on an automatic driving vehicle has a certain observation blind area, usually a relatively short-distance area on the left and right sides of the vehicle, and cannot acquire speed information of a target in the blind area.
In order to solve the above problems, an embodiment of the present invention provides a method for estimating a target speed, which calculates the speed of the target by two different speed estimation methods based on characteristics of different sensing modules of the vehicle, and then selects one of the calculated speeds as the target speed according to different positions of the target relative to the vehicle.
In a first aspect of the embodiment of the present invention, as shown in fig. 1, a target speed estimation method is provided, which may include the following steps:
s101, calculating a first to-be-selected speed and a second to-be-selected speed according to a first speed calculation mode and a second speed calculation mode respectively;
S102, judging the position of a target relative to a vehicle;
S103, determining the first candidate speed or the second candidate speed as the speed of the target according to the position of the target relative to the vehicle:
When the target is determined to be positioned in the area inside the first boundary taking the own vehicle as the symmetry center, determining a second candidate speed obtained according to the second speed calculation mode as the speed of the target;
In particular implementations, "outside of the first boundary" may be set to include the first boundary itself while "inside of the first boundary" is set to not include the first boundary itself, or "inside of the first boundary" may be set to include the first boundary itself while "outside of the first boundary" is set to not include the first boundary itself.
In specific implementation, when it is determined according to basic characteristics, calibration data, test data, and the like of the lidar that the original point cloud obtained by the lidar is relatively complete compared with the theoretical value (for example, the ratio of the original point cloud to the theoretical value exceeds a preset ratio threshold), the error of the final output result is relatively small compared with the real situation of the target, whereas when the target is located in an area outside the certain boundary, the original point cloud obtained by the lidar is not sufficiently complete compared with the theoretical value (for example, the ratio of the original point cloud to the theoretical value is lower than a preset ratio threshold), and the error of the final output result is relatively large compared with the real situation of the target, in this case, the boundary may be set as the first boundary in the above embodiment.
For a specific vehicle and a laser radar, the first boundary dynamically changes along with the movement of the vehicle, but the relative position of the first boundary and the vehicle is relatively stable, so that the first boundary can be determined in advance under the vehicle coordinate system according to the basic characteristics, calibration data, test data and the like of the laser radar.
When the target is positioned in the area outside the first boundary, the information errors of the target length, width, height, center point and the like of the result output by the laser radar are larger, and the calculation result of the first speed calculation mode is taken as the target speed.
As an alternative embodiment, the first boundary may be a closed boundary, for example, the first boundary is a closed boundary with the origin of the vehicle coordinate system as the symmetry center, such as a circular closed boundary, an elliptical closed boundary, or a rectangular closed boundary, or a closed boundary with other shapes, as shown in fig. 2, an elliptical boundary 300 with the origin of the vehicle coordinate system as the symmetry center, and the calculation result of the second speed calculation mode is taken as the target speed when the target 200 is within the elliptical boundary 300, and the calculation result of the first speed calculation mode is taken as the target speed when the target is located in an area other than the elliptical boundary 300, with respect to the position of the vehicle 100.
As an alternative embodiment, the first boundary may also be a non-closed boundary, for example, the first boundary may be two linear boundaries with the origin of the vehicle coordinate system as the symmetry center, the area in the middle of the two linear boundaries is inside the first boundary, and the area outside the two linear boundaries is outside the first boundary.
The two linear boundaries may be a first linear boundary 1 and a second linear boundary 2, which are respectively located in front of and behind the vehicle 100 and perpendicular to the traveling direction of the vehicle, as shown in fig. 5, the vehicle coordinate system is established with the center of the rear axis of the vehicle 100 as the origin, the x-axis and the y-axis respectively represent the vertical axis and the horizontal axis of the vehicle coordinate system, and the first linear boundary 1 is an exemplary linear boundary located in front of the vehicle by 10m (i.e., +10m in the x-axis and parallel to the y-axis), and the second linear boundary 2 is an exemplary linear boundary located in back of the vehicle by 10m (i.e., -10m in the x-axis and parallel to the y-axis).
In specific implementation, the first straight line boundary 1 and the second straight line boundary 2 are determined according to the complete condition of the point cloud detected by the laser radar on the target. For example, according to basic characteristics, calibration data and a large amount of test data of the laser radar, when the target is positioned within 10m in front of or behind the vehicle, information such as the length, width, height, center point and the like of the resultant target output by the laser radar is accurate, and when the target is positioned outside 10m in front of or behind the vehicle, information such as the length, width, height, center point and the like of the resultant target output by the laser radar is large, in which case the first straight boundary 1 and the second straight boundary 2 can be set to be positioned within 10m in front of or behind the vehicle, respectively.
Specifically, as shown in fig. 5, with respect to the vehicle 100, the area outside the first straight boundary 1 and the second straight boundary 2 is a first area a, the area inside the first straight boundary 1 and the second straight boundary 2 is a second area B, and when the target 200 (id#1) is located in the first area a, the speed calculated by the first speed calculation method is used as the final target estimated speed, and when the target 200 (id#1) is located in the second area B, the speed calculated by the second speed calculation method is used as the final target estimated speed.
At present, the laser radar commonly used in the market tracks the target by taking the central point of the detected target as a tracking point, and is influenced by the observable position, the target clustering effect and other factors, when the target is close to the vehicle, the laser radar can continuously, stably and accurately detect the central point of the target, and when the target is far from the vehicle, the central point detected by the laser radar cannot be continuous, is not stable enough (such as the tracking point is lost) or is inaccurate, and then the obtained target tracking result is inaccurate.
In view of this problem, a second aspect of the embodiments of the present invention provides an object tracking method for a lidar, including:
Step 1, judging the position of a target relative to a vehicle;
The step 2 of determining a target tracking point according to the position of the target relative to the vehicle, specifically comprising taking the nearest point of the target detected by the laser radar as the target tracking point when the target is positioned in an area outside a second boundary taking the vehicle as a symmetry center, or taking the central point of the target detected by the laser radar as the target tracking point when the target is positioned in an area inside the second boundary taking the vehicle as a symmetry center;
and step 3, tracking the target based on the target tracking point.
Specifically, for a far target, although the laser radar cannot continuously or accurately detect the center point of the target, the visible edge position of the target obtained by the laser radar is relatively stable, and the original point cloud data at the edge of the target can be obtained, so that the nearest point of the target to the vehicle can be obtained, therefore, the nearest point of the target detected by the laser radar can be used as the target tracking point, and the obtained target tracking result can be more accurate and stable due to the fact that the directly measured original point cloud data is utilized.
The above-mentioned target tracking method for laser radar may be applied to calculating the first candidate speed in the first speed calculation mode, as shown in fig. 3, and the calculating the first candidate speed according to the first speed calculation mode includes:
s201, judging the position of a target relative to a vehicle;
S202, determining a target tracking point according to the position of the target relative to the own vehicle:
When the target is determined to be positioned in the area which is positioned in the second boundary with the own vehicle as the symmetry center, the closest point of the target detected by the laser radar is used as a target tracking point;
and S203, estimating and obtaining a first speed to be selected based on the target tracking point.
In particular implementations, "outside of the second boundary" may be set to include the second boundary itself while "inside of the second boundary" is set to not include the second boundary itself, or "inside of the second boundary" may be set to include the second boundary itself while "outside of the second boundary" is set to not include the second boundary itself.
In particular, when it is determined that the target is located in an area within a certain boundary based on basic characteristics of the laser radar, calibration data, test data, and the like, the laser radar can continuously and accurately detect the center point of the target, whereas when the target is located in an area outside a certain boundary, the laser radar cannot continuously or accurately detect the center point of the target, in which case the boundary may be set as the second boundary in the above-described embodiment.
For a specific vehicle and the laser radar, the second boundary dynamically changes along with the movement of the vehicle, but the relative position of the second boundary and the vehicle is relatively stable, so that the second boundary can be determined in advance under the vehicle coordinate system according to the basic characteristics, calibration data, test data and the like of the laser radar.
The laser radar can continuously and accurately detect the center point of the target when the target is positioned in the area inside the second boundary, wherein the center point of the target detected by the laser radar is used as the target tracking point, and the laser radar cannot continuously or accurately detect the center point of the target when the target is positioned in the area outside the second boundary, wherein the nearest point of the target detected by the laser radar is used as the target tracking point.
As an alternative embodiment, the second boundary may be a closed boundary, for example, the second boundary may be a closed boundary with the origin of the vehicle coordinate system as a symmetry center, such as a circular closed boundary, an elliptical closed boundary, or a rectangular closed boundary, or a closed boundary with another shape.
As an alternative embodiment, the second boundary may be a non-closed boundary, for example, the second boundary may be two linear boundaries with the origin of the vehicle coordinate system as the symmetry center, the area in the middle of the two linear boundaries is the area inside the second boundary, and the area outside the two linear boundaries is the area outside the second boundary.
The two linear boundaries corresponding to the second boundary may be a third linear boundary 3 and a fourth linear boundary 4, which are respectively located in front of and behind the vehicle 100 and perpendicular to the traveling direction of the vehicle, as shown in fig. 6, the vehicle coordinate system is established with the center of the rear axis of the vehicle 100 as the origin, the x-axis and the y-axis respectively represent the vertical axis and the horizontal axis of the vehicle coordinate system, and the third linear boundary 3 is an exemplary linear boundary located in front of the vehicle by 2m (i.e., in the x-axis by +2m and parallel to the y-axis), and the fourth linear boundary 4 is an exemplary linear boundary located in back of the vehicle by 2m (i.e., in the x-axis by-2 m and parallel to the y-axis).
In specific implementation, the third straight boundary 3 and the fourth straight boundary 4 are determined according to the stability condition of the laser radar on the target set tracking point. For example, according to the basic characteristics of the laser radar, the calibration data and the large amount of test data, it is shown that the laser radar can continuously and accurately detect the center point of the target when the target is located in an area within 2m in front of or behind the host vehicle, and thus the target tracking result by using the center point as the tracking point is also continuous and accurate, whereas the laser radar cannot continuously or accurately detect the center point of the target when the target is located in an area other than 2m in front of or behind the host vehicle, and thus the target tracking result by using the center point as the tracking point is also insufficiently continuous and accurate, and in this case, the third straight boundary 3 and the fourth straight boundary 4 can be set to be located in front of or behind the host vehicle by 2m, respectively.
For the scenario shown in fig. 6, considering that the target may move back and forth (e.g., the target vehicle suddenly decelerates, accelerates, changes lanes, etc.) near the second boundary (the third straight boundary 3, the fourth straight boundary 4), according to the aforementioned target tracking method for the laser radar, the tracking point needs to be switched every time the target moves from the area outside or inside the second boundary to the area inside or outside the second boundary, however, the kalman filter needs to be reinitialized every time the tracking point is switched, which affects the tracking effect and the tracking efficiency to some extent.
In order to avoid the situation that tracking points are switched back and forth at boundaries, the embodiment of the invention provides a more optimized target tracking method for a laser radar, which comprises the following steps:
When it is determined that the target enters the region between the third straight boundary 3 and the fourth straight boundary 4 from the region outside the third straight boundary 3 located in front of the host vehicle or from the region outside the fourth straight boundary 4 located behind the host vehicle, and then enters the region outside the fourth straight boundary 4 located behind the host vehicle or enters the region outside the third straight boundary 3 located in front of the host vehicle, when the target does not leave the region between the third straight boundary 3 and the fourth straight boundary 4, the target tracking point before entering the region is still adopted, and when the target leaves the region, the target tracking point is updated to be the closest point of the target currently detected by the laser radar.
Specifically, as shown in fig. 6, the region other than the third straight boundary 3 is the first region, the region between the third straight boundary 3 and the fourth straight boundary 4 is the second region, and the region other than the fourth straight boundary 4 is the third region.
For the target 200 labeled id#0, when the target 200 is generated as a target (i.e., a subsequent tracking is performed as an effective obstacle target), since the target is located in the third area, the tracking point is selected as the closest point of the target detected by the laser radar, i.e., a dot marked on the top of a rectangular block corresponding to id#0 in the figure.
For the target 200 denoted by id#1, when the target 200 is generated as a target, since the target is located in the first area, the tracking point is also selected as the closest point to the target detected by the lidar, that is, a dot indicated at the top of a rectangular block corresponding to id#1 in the figure.
For the object 200, numbered id #2, the object enters the second area from the third area to the first area during tracking. In the third area, the target tracking point is the closest point of the target detected by the laser radar, namely, a dot marked on the top end of a rectangular square corresponding to id#2 in the figure (for example, the center point of the head of a target vehicle), when the target passes through the fourth linear boundary 4 and enters the second area but does not reach the third linear boundary 3, the target tracking point is still the closest point of the target detected by the laser radar (the dot marked on the top end of the rectangular square corresponding to id#2 in the figure), after the target passes through the third linear boundary 3 and enters the first area, the target tracking point is switched to the closest point of the target detected by the current laser radar, namely, the dot marked on the bottom end of the rectangular square corresponding to id#2 in the figure (for example, the center point of the tail of the target vehicle), and if the target enters the second area again and then enters the third area, the tracking point is switched at the fourth linear boundary 4. By the arrangement, the situation that the target moves back and forth near the third linear boundary 3 or the fourth linear boundary 4 but cannot completely enter other areas can be filtered, the back and forth switching of tracking points caused by the situation is avoided, and tracking efficiency and accuracy are ensured.
As an optional embodiment, the estimating of the first candidate speed based on the target tracking point in the foregoing embodiment may be to use point cloud data of the target tracking point updated in real time by using a laser radar, and/or use a kalman filter to estimate the first candidate speed by using a millimeter wave radar to update target position and speed information in real time. The step may adopt a multi-sensor information fusion technology and a target tracking technology commonly used in the art, and the embodiments of the present invention are not repeated.
For the object located within the first boundary, a second candidate speed is calculated according to a second speed calculation mode, as shown in fig. 4, and specifically includes:
S301, calculating the moving speed of each target frame corner point and each target tracking point by utilizing the position data of the target frame corner point and the target tracking point obtained by the laser radar;
s302, estimating a second candidate speed according to the moving speed of each target frame corner point and each target tracking point.
As shown in fig. 8, in the vehicle coordinate system of the vehicle 100, the target 200 (gray rectangular box) is located in an area outside the second boundary, the target tracking point is set as the closest point of the target detected by the laser radar, the target frame 7 detected by the laser radar on the target 200 is a rectangular frame shown by a dotted line, four target frame corner points 5 are left upper, left lower, right upper and right lower, and a round point at the bottom edge center of the rectangular frame is the target tracking point 6. If the target 200 is located in an area within the second boundary, the target tracking point 6 is a target center point detected by the laser radar.
As shown in fig. 9, the position data of each target frame corner 5 (upper left corner, upper right corner, lower left corner, lower right corner) and the position data of the target tracking point 6 obtained by the lidar at a plurality of consecutive times (fig. 9 is consecutive 5 time stamps) are recorded.
As an alternative embodiment, step S301 may calculate the moving speed of the target frame corner and the target tracking point of each time stamp according to the position data of the target frame corner and the target tracking point of the continuous plurality of time stamps using the following uniform linear motion model:
wherein N is an integer greater than or equal to 2, representing an nth timestamp;
Vx is the X-axis velocity component of the target frame corner/target tracking point, vy is the Y-axis velocity component of the target frame corner/target tracking point, X N is the X-axis position data of the target frame corner/target tracking point at the Nth time stamp, Y N is the Y-axis position data of the target frame corner/target tracking point at the Nth time stamp, and T N is the time of the Nth time stamp;
X 0 is X-axis position data of a target frame corner/target tracking point acquired by the first time stamp in the N continuous time stamps, Y 0 is Y-axis position data of the target frame corner/target tracking point acquired by the first time stamp in the N continuous time stamps, and T 0 is time corresponding to the first time stamp in the N continuous time stamps.
In the uniform linear motion model, at least data acquired by 3 continuous time stamps (N is an integer greater than or equal to 2) are selected for calculation, so that adverse effects of the same system errors possibly existing in data acquired by adjacent time stamps can be filtered, and a more accurate result is obtained.
In practical implementation, the target frame 7 obtained by the laser radar may be a non-rectangular frame, and the corner points of the target frame may be not limited to the four corner points. Optionally, several target frame corner points may be arbitrarily selected from all the target frame corner points for calculating the second candidate speed.
As an alternative embodiment, step S302 may be to determine at least one of the moving speeds of the corner points of the target frame and the target tracking point as the second candidate speed, for example, determine the moving speed that most reflects the current speed of the target from the moving speeds of the four corner points and the target tracking point as the second candidate speed.
Optionally, step S302 determines a second candidate speed closest to the previous timestamp from the moving speeds of the target frame corner point and the target tracking point of the current timestamp as the second candidate speed of the current timestamp, where the second candidate speed of the initial timestamp is the speed of the target when the target is newly established. The method comprises the steps of adopting an iterative calculation mode, taking the second candidate speed closest to the previous timestamp from the moving speeds of the target frame corner point and the target tracking point of the current timestamp as the second candidate speed of the current timestamp, and filtering speed estimation deviation caused by jump of the target frame corner point detected by the laser radar to obtain a smoother speed estimation value.
As shown in fig. 9, after storing position data of the target frame corner points and the target tracking points acquired by the lidar at 5 consecutive moments, the moving speed (including the speed component of the X axis and the speed component of the Y axis) of each target frame corner point and the target tracking point at the time T-1 is calculated respectively by using a uniform linear motion model:
the moving speed of the target tracking point is vxrel _0 and vyrel_0
The moving speed of the upper left corner point is vxrel _1 and vyrel_1
The moving speed of the upper right corner point is vxrel _2 and vyrel_2
The moving speed of the left lower corner point is vxrel _3 and vyrel_3
The moving speed of the right lower corner point is vxrel _4 and vyrel_4
After that, one set of speeds is selected from the five sets of speed data corresponding to the time T-1 as the second candidate speed at the time T, for example, the second candidate speed (vxrel, vyrel) closest to the time T-1 may be the second candidate speed (vxrel ', vyrel') at the time T. The second candidate speed at the time T-1 is determined based on the second candidate speed at the time T-2 according to the above manner, i.e., the second candidate speed at each time is determined in an iterative manner. The second candidate speed of the initial timestamp is a speed component of the X axis and the Y axis when the target is established as a new target, and may specifically be obtained by calculation according to a uniform linear motion model, which is not described herein.
In the embodiment of the invention, the new establishment of the target refers to establishing a new target by using the obtained measurement information when determining that the virtual obstacle target meets the requirement of a new target generation rule (for example, the distance from the vehicle transverse direction or the longitudinal direction reaches a safety threshold) according to the obtained measurement values (for example, the laser radar, the millimeter wave radar or the vision sensor measurement data) of a plurality of associated sensors for a certain virtual obstacle target.
According to the embodiment of the invention, a target speed calculation mode of a tracking point determined based on the laser radar and a target speed calculation mode of displacement information of a target frame corner point and a center point measured based on the laser radar are adopted, speed estimation is respectively carried out on an obstacle target, then a speed estimation result is selected as the final estimated speed of the target according to different positions of the obstacle target relative to a vehicle, and the calculated target speed is ensured to be higher in precision by selecting the output target speed, so that the technical problem that inaccurate estimation is caused by the fact that the laser radar greatly influences the target speed estimation on the position of the target is solved. In addition, by utilizing the characteristic of 360-degree full coverage scanning of the laser radar on the periphery of the vehicle, the embodiment of the invention solves the problem of blind area coverage of the millimeter wave radar from the aspect of hardware configuration.
Optionally, in the embodiment of the present invention, the speed estimation of the newly built target adopts different strategies according to the associated sensor measurement category, for example, when the measurement value matched with the virtual target is measured by using the laser radar, the speed of the newly built target is estimated by using the displacement and time variation of the virtual target, and when the measurement value matched with the virtual target is measured by using the millimeter wave radar, the speed measurement value of the millimeter wave radar can also be used as the speed estimation value of the newly built target.
Referring to fig. 10, in a second aspect of the embodiment of the present invention, there is provided a target speed estimating apparatus including:
the first speed calculating unit is used for calculating a first speed to be selected according to a first speed calculating mode;
The second speed calculation unit is used for calculating a second to-be-selected speed according to a second speed calculation mode;
a speed determining unit, configured to determine, when the target is located in an area outside a first boundary with the own vehicle as a center of symmetry, a first speed to be selected as a speed of the target, and conversely determine a second speed to be selected as a speed of the target;
wherein the first speed calculation unit includes:
The tracking point determining module is used for determining that when the target is positioned in an area outside a second boundary taking the vehicle as a symmetry center, the closest point of the target detected by the laser radar is used as a target tracking point;
the first speed estimation module is used for estimating a first speed to be selected based on the target tracking point;
Wherein the second speed calculation unit includes:
the target point speed calculation module is used for calculating the moving speed of each target frame angular point and each target tracking point by utilizing the displacement information of the target frame angular point and the target tracking point obtained by the laser radar;
And the second speed estimation module is used for estimating a second candidate speed according to the moving speed of each target frame corner point and each target tracking point.
The target speed estimation device of the embodiment of the present invention may specifically adopt the target speed estimation technique disclosed by the target speed estimation method of the first aspect of the present invention to perform the estimation of the target speed of the obstacle, which is not described herein, please refer to the content of the target speed estimation method disclosed by the target speed estimation method of the first aspect of the present invention.
In a third aspect of the embodiment of the present invention, a control device is provided, including at least one processor, and a memory communicatively connected to the at least one processor, where the memory stores instructions executable by the at least one processor, and the instructions are executed by the at least one processor, so that the at least one processor can execute the target speed estimation method according to the first aspect of the embodiment of the present invention to perform the target speed estimation of the obstacle, and the target speed estimation method is not repeated.
In a fourth aspect of the embodiments of the present invention, a computer readable storage medium is provided, which includes a program or an instruction, where when the program or the instruction runs on a computer, the target speed estimation method according to the first aspect of the embodiments of the present invention is implemented to perform the estimation of the target speed of the obstacle, and the target speed estimation method is not described herein.
In a fifth aspect of the embodiments of the present invention, a computer program product including instructions is provided, where the computer program product when executed on a computer causes the computer to execute the target speed estimation method according to the first aspect of the embodiments of the present invention to perform target speed estimation of an obstacle, and the target speed estimation method is not described in detail.
A sixth aspect of the embodiment of the present invention provides a vehicle, including the control apparatus according to the third aspect of the embodiment of the present invention.
According to the invention, two different speed estimation modes are adopted to respectively estimate the speed of the obstacle target, and then one speed estimation result is selected as the final estimated speed of the target according to the different positions of the obstacle target relative to the vehicle, so that the problem that the automatic driving vehicle cannot acquire the speed information of the target in the millimeter wave radar blind area is solved, the problem that the laser radar measurement result is greatly influenced by the position is solved, and the accuracy of the speed estimation of the target is improved.
The foregoing is merely a preferred embodiment of the present invention and it should be noted that modifications and adaptations to those skilled in the art may be made without departing from the principles of the present invention, which are intended to be comprehended within the scope of the present invention.
Claims (18)
1. A method of estimating a target speed, comprising the steps of:
Calculating a first to-be-selected speed and a second to-be-selected speed according to the first speed calculation mode and the second speed calculation mode respectively;
When the target is determined to be positioned in an area outside a first boundary taking the vehicle as a symmetry center, determining a first speed to be selected obtained according to a first speed calculation mode as the speed of the target;
when the target is determined to be positioned in the area which is positioned in the first boundary with the vehicle as the symmetry center, determining a second candidate speed obtained according to the second speed calculation mode as the speed of the target;
Wherein, calculate the first speed of choosing according to the first speed calculation mode, include:
When the target is determined to be positioned in the area which is positioned in the second boundary with the own vehicle as the symmetry center, the closest point of the target detected by the laser radar is used as a target tracking point;
estimating a first speed to be selected based on the target tracking point;
the second candidate speed is calculated according to a second speed calculation mode, and the method comprises the following steps:
Calculating the moving speed of each target frame corner point and each target tracking point by utilizing the position data of the target frame corner point and the target tracking point obtained by the laser radar;
And estimating a second candidate speed according to the moving speed of each target frame corner point and each target tracking point.
2. The method of claim 1, wherein the first boundary is a circular closed boundary, an elliptical closed boundary, or a rectangular closed boundary with an origin of a vehicle coordinate system as a center of symmetry.
3. The target speed estimation method according to claim 1, wherein the first boundary includes two linear boundaries centered on an origin of the own vehicle coordinate system.
4. The target speed estimation method according to claim 3, wherein the two linear boundaries include a first linear boundary and a second linear boundary located respectively in front of and behind the own vehicle and perpendicular to a traveling direction of the own vehicle.
5. The method of estimating a target speed according to claim 1, wherein the second boundary is a circular closed boundary, an elliptical closed boundary, or a rectangular closed boundary with an origin of a vehicle coordinate system as a center of symmetry.
6. The target speed estimation method according to claim 1, wherein the second boundary includes two linear boundaries centered on the origin of the own vehicle coordinate system.
7. The target speed estimation method according to claim 6, wherein the two linear boundaries include a third linear boundary and a fourth linear boundary located respectively in front of and behind the own vehicle and perpendicular to a traveling direction of the own vehicle.
8. The method according to claim 7, wherein the step of determining that the target is located in an area other than the second boundary having the vehicle as the center of symmetry, using the closest point of the target detected by the laser radar as the target tracking point, or determining that the target is located in an area other than the second boundary having the vehicle as the center of symmetry, using the center point of the target detected by the laser radar as the target tracking point, comprises:
Determining that the object enters the region between the third straight line boundary and the fourth straight line boundary from the region outside the third straight line boundary in front of the vehicle or from the region outside the fourth straight line boundary in back of the vehicle, and then enters the region outside the fourth straight line boundary in back of the vehicle or enters the region outside the third straight line boundary in front of the vehicle,
When the target leaves the area, the target tracking point is updated to be the nearest point of the target currently detected by the laser radar.
9. The method of estimating a target speed according to claim 1, wherein estimating a first candidate speed based on the target tracking point comprises:
And estimating and obtaining a first speed to be selected by utilizing point cloud data of a target tracking point updated in real time by the laser radar and/or target position and speed information updated in real time by the millimeter wave radar and utilizing a Kalman filter.
10. The method of estimating a target speed according to claim 1, wherein calculating a moving speed of each of the target frame corner point and the target tracking point using position data of the target frame corner point and the target tracking point obtained by the laser radar, comprises:
and calculating the moving speed of the target frame corner point and the target tracking point of each time stamp according to the position data of the target frame corner point and the target tracking point of the continuous multiple time stamps by using the uniform linear motion model.
11. The target speed estimation method of claim 10 wherein the continuous plurality of time stamps is at least 3 time stamps in succession.
12. The method for estimating a target speed according to claim 1, wherein estimating the second candidate speed according to the moving speed of each of the target frame corner point and the target tracking point comprises:
and determining at least one of the moving speeds of each target frame corner and each target tracking point as a second candidate speed.
13. The method of estimating a target speed according to claim 12, wherein determining at least one of the moving speeds of each of the target frame corner point and the target tracking point as the second candidate speed comprises:
and determining a second to-be-selected speed closest to the previous timestamp in the moving speeds of the target frame corner point and the target tracking point of the current timestamp as the second to-be-selected speed of the current timestamp, wherein the second to-be-selected speed of the initial timestamp is the speed of the target when the target is newly established.
14. A target speed estimation device, characterized by comprising:
the first speed calculating unit is used for calculating a first speed to be selected according to a first speed calculating mode;
The second speed calculation unit is used for calculating a second to-be-selected speed according to a second speed calculation mode;
a speed determining unit, configured to determine, when the target is located in an area outside a first boundary with the own vehicle as a center of symmetry, a first speed to be selected as a speed of the target, and conversely determine a second speed to be selected as a speed of the target;
wherein the first speed calculation unit includes:
The tracking point determining module is used for determining that when the target is positioned in an area outside a second boundary taking the vehicle as a symmetry center, the closest point of the target detected by the laser radar is used as a target tracking point;
the first speed estimation module is used for estimating a first speed to be selected based on the target tracking point;
Wherein the second speed calculation unit includes:
the target point speed calculation module is used for calculating the moving speed of each target frame angular point and each target tracking point by utilizing the position data of the target frame angular point and the target tracking point obtained by the laser radar;
And the second speed estimation module is used for estimating a second candidate speed according to the moving speed of each target frame corner point and each target tracking point.
15. A control device comprising at least one processor, and a memory communicatively coupled to the at least one processor, wherein the memory stores instructions executable by the at least one processor to enable the at least one processor to perform the target speed estimation method of any one of claims 1-13.
16. A computer readable storage medium comprising a program or instructions which, when run on a computer, implements the target speed estimation method according to any one of claims 1-13.
17. A computer program product containing instructions which, when run on a computer, cause the computer to perform the target speed estimation method according to any one of claims 1-13.
18. A vehicle comprising the control apparatus according to claim 15.
Applications Claiming Priority (3)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN2021103895278 | 2021-04-12 | ||
| CN202110389527.8A CN115201804B (en) | 2021-04-12 | 2021-04-12 | Target speed estimation method, device and storage medium |
| PCT/CN2021/117101 WO2022217819A1 (en) | 2021-04-12 | 2021-11-11 | Target speed estimation method and apparatus, and storage medium |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114375406A CN114375406A (en) | 2022-04-19 |
| CN114375406B true CN114375406B (en) | 2025-05-06 |
Family
ID=81141420
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202180005033.3A Active CN114375406B (en) | 2021-04-12 | 2021-11-11 | Target speed estimation method, device and storage medium |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114375406B (en) |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109080628A (en) * | 2017-06-14 | 2018-12-25 | 丰田自动车株式会社 | Target determines equipment and driving assistance system |
| CN109709530A (en) * | 2017-10-26 | 2019-05-03 | 株式会社小糸制作所 | Sensor-based system and vehicle |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR102569904B1 (en) * | 2018-12-18 | 2023-08-24 | 현대자동차주식회사 | Apparatus and method for tracking target vehicle and vehicle including the same |
-
2021
- 2021-11-11 CN CN202180005033.3A patent/CN114375406B/en active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109080628A (en) * | 2017-06-14 | 2018-12-25 | 丰田自动车株式会社 | Target determines equipment and driving assistance system |
| CN109709530A (en) * | 2017-10-26 | 2019-05-03 | 株式会社小糸制作所 | Sensor-based system and vehicle |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114375406A (en) | 2022-04-19 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110705458B (en) | Boundary detection method and device | |
| CN114556442B (en) | Three-dimensional point cloud segmentation method and device, and movable platform | |
| CN113432533B (en) | Robot positioning method and device, robot and storage medium | |
| WO2018177026A1 (en) | Device and method for determining road edge | |
| CN112346463A (en) | Unmanned vehicle path planning method based on speed sampling | |
| CN115151836A (en) | Method for detecting a moving object in the surroundings of a vehicle and motor vehicle | |
| CN112465193B (en) | Parameter optimization method and device for multi-sensor data fusion | |
| CN114155511A (en) | Environmental information acquisition method for automatically driving automobile on public road | |
| CN113494915A (en) | Vehicle transverse positioning method, device and system | |
| CN116381632B (en) | Self-calibration method and device for radar roll angle and storage medium | |
| JP7344744B2 (en) | Roadside edge detection method and roadside edge detection device | |
| CN114375406B (en) | Target speed estimation method, device and storage medium | |
| CN115953328A (en) | A target correction method, system and electronic equipment | |
| US20250076880A1 (en) | High-definition mapping | |
| CN112902911A (en) | Monocular camera-based distance measurement method, device, equipment and storage medium | |
| JP2021060943A (en) | Occupation map creation method and occupation map creation device | |
| CN114185026A (en) | External parameter calibration method and device for vehicle-mounted multi-laser radar and electronic equipment | |
| CN117657219A (en) | An automatic driving vehicle control method and device at an intersection | |
| CN120080854A (en) | Method, device, equipment and medium for identifying the motion state of a preceding vehicle | |
| CN115327528B (en) | Target stable tracking method, device, radar and storage medium | |
| CN117968682A (en) | Dynamic point cloud removal method based on multi-line laser radar and inertial measurement unit | |
| CN112166341A (en) | Speed determination method, apparatus and storage medium | |
| WO2022217819A1 (en) | Target speed estimation method and apparatus, and storage medium | |
| KR20230132325A (en) | Method And Apparatus for Detecting Lane Using LiDAR | |
| CN113203424A (en) | Multi-sensor data fusion method and device and related equipment |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |