CN111381249B - Method and device for calculating course angle of obstacle - Google Patents
Method and device for calculating course angle of obstacle Download PDFInfo
- Publication number
- CN111381249B CN111381249B CN202010238096.0A CN202010238096A CN111381249B CN 111381249 B CN111381249 B CN 111381249B CN 202010238096 A CN202010238096 A CN 202010238096A CN 111381249 B CN111381249 B CN 111381249B
- Authority
- CN
- China
- Prior art keywords
- course angle
- value
- weight coefficient
- obstacle
- reference value
- 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
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/18—Complex mathematical operations for evaluating statistical data, e.g. average values, frequency distributions, probability functions, regression analysis
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/70—Determining position or orientation of objects or cameras
- G06T7/77—Determining position or orientation of objects or cameras using statistical methods
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10028—Range image; Depth image; 3D point clouds
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/30—Subject of image; Context of image processing
- G06T2207/30248—Vehicle exterior or interior
- G06T2207/30252—Vehicle exterior; Vicinity of vehicle
- G06T2207/30261—Obstacle
Abstract
The invention provides a method and a device for calculating a heading angle of an obstacle, which are applied to the technical field of automobiles. According to the method, the influence degree of the first course angle component and the second course angle component in the final calculation result is adjusted through the first weight coefficient and the second weight coefficient, so that more accurate course angle components account for more proportion, the advantages of the existing calculation method can be fully played, and the accuracy of the calculation result of the course angle of the obstacle is improved.
Description
Technical Field
The invention belongs to the technical field of automobiles, and particularly relates to a method and a device for calculating a heading angle of an obstacle.
Background
In the technical field of automatic driving, accurate prediction of obstacle behaviors and obstacle tracks is the key of efficient and safe driving of an automatic driving vehicle, and the heading angle of an obstacle has important significance for accurately predicting the behaviors and the tracks of the obstacle.
Generally, there are two methods for calculating the heading angle of an obstacle, the first method is implemented based on an obstacle OBB (Oriented Bounding Box), and the second method is implemented based on preset movement parameters of the obstacle, such as the position and the speed of the obstacle.
The course angle calculation method based on the obstacle OBB can realize course angle calculation of a static obstacle and a moving obstacle, but the method excessively depends on the scanning result of the laser radar on the obstacle, and the calculation result is often greatly deviated from an actual value under the condition that the obstacle cannot be completely scanned; although the course angle calculation method based on the preset movement parameters of the obstacle can effectively overcome the defects of the first calculation method, the course angle of the static obstacle cannot be calculated.
Disclosure of Invention
In view of the above, the present invention provides a method and an apparatus for calculating an obstacle course angle, which fuse calculation results of different existing calculation methods, fully exert the advantages of the existing calculation methods, and improve the accuracy of the calculation result of the obstacle course angle, and the specific scheme is as follows:
in a first aspect, the present invention provides a method for calculating a heading angle of an obstacle, including:
acquiring at least two first course angle reference values and at least two second course angle reference values of a target obstacle, wherein the first course angle reference values are acquired based on an oriented bounding box OBB of the target obstacle, the second course angle reference values are acquired based on preset motion parameters of the target obstacle, and the first course angle reference values and the second course angle reference values are equal in number;
determining a first course angle component and a first weight coefficient corresponding to the first course angle component according to each first course angle reference value, wherein the first weight coefficient is negatively correlated with the variance of each first course angle reference value and is used for representing the influence degree of the first course angle component on the course angle of the obstacle;
determining a second course angle component and a second weight coefficient corresponding to the second course angle component according to each second course angle reference value, wherein the second weight coefficient is negatively related to the variance of each second course angle reference value and is used for representing the influence degree of the second course angle component on the course angle of the obstacle, and the sum of the second weight coefficient and the first weight coefficient is 1;
and calculating to obtain the course angle of the target obstacle according to the first course angle component, the first weight coefficient, the second course angle component and the second weight coefficient.
Optionally, the determining a first heading angle component according to each first heading angle reference value includes:
correcting the first course angle reference value in the detection period based on the difference value and the magnitude relation between the first course angle reference value and the second course angle reference value obtained in the same detection period to obtain a first course angle correction value;
and determining a first course angle component according to the first course angle correction value.
Optionally, the correcting the first course angle reference value in the detection period based on the difference and the magnitude relationship between the first course angle reference value and the second course angle reference value obtained in the same detection period to obtain a first course angle correction value includes:
determining a corresponding correction formula in the following four correction formulas according to the difference value and the magnitude relation of the first course angle reference value and the second course angle reference value obtained in the same detection period to obtain a target correction formula:
wherein the content of the first and second substances,
a _ OBB represents a first heading angle reference value;
a _ mo represents a second heading angle reference value;
a _ OBB _ X represents a first course angle correction value;
m represents a first preset threshold;
n represents a second preset threshold;
and substituting the first course angle reference value into a target correction formula, and taking the obtained correction result as a first course angle correction value.
Optionally, the determining a first heading angle component according to each first heading angle correction value includes:
acquiring a weight coefficient corresponding to each first course angle correction value;
respectively calculating the product of each first course angle correction value and the corresponding weight coefficient to obtain a corresponding first course angle weighted value;
and taking the average value or the truncated average value of the weighted values of the first course angle as a first course angle component.
Optionally, the determining a second heading angle component according to each second heading angle reference value includes:
acquiring a weight coefficient corresponding to each second course angle reference value;
respectively calculating the product of each second course angle reference value and the corresponding weight coefficient to obtain a corresponding second course angle weighted value;
and taking the average value or the truncated average value of the weighted values of the second course angle as a second course angle component.
Optionally, the process of determining the first weight coefficient and the second weight coefficient includes:
calculating to obtain a first variance value according to the first course angle component and each first course angle correction value;
calculating to obtain a second variance value according to the second course angle component and each second course angle reference value;
substituting the first variance value and the second variance value into the following formula to obtain a first weight coefficient:
substituting the first variance value and the second variance value into the following formula to obtain a second weight coefficient:
wherein the content of the first and second substances,
x1representing a first weight coefficient;
x2represents a second weight coefficient;
pOBBrepresenting a first variance value;
pmorepresenting the second variance value.
Optionally, the obtaining a first heading angle reference value of the target obstacle includes:
in any detection period, point cloud fed back by the laser radar is obtained;
dividing the point clouds according to a preset division rule to obtain at least one point cloud cluster, wherein one point cloud cluster corresponds to one obstacle;
determining a target point cloud cluster in at least one point cloud cluster, and obtaining a target obstacle corresponding to the target point cloud cluster;
constructing an OBB of the target obstacle according to the target point cloud cluster;
and obtaining a first course angle reference value based on the OBB of the target obstacle.
Optionally, the process of obtaining the second heading angle reference value of the target obstacle includes:
acquiring the running speed and the yaw rate of the vehicle;
and determining a second course angle reference value of the target obstacle according to the OBB of the target obstacle, the running speed and the yaw rate.
Optionally, the determining a second heading angle reference value of the target obstacle according to the OBB of the target obstacle, the driving speed, and the yaw rate includes:
determining the relative movement speed of the target obstacle in a preset coordinate system and the position parameters of the target obstacle according to the OBB of the target obstacle, wherein the preset coordinate system is set based on a specified central position of a vehicle, and the position parameters comprise an x-axis position and a y-axis position of the target obstacle in the preset coordinate system;
substituting the x-axis speed component and the y-axis speed component of the relative movement speed in the preset coordinate system, the x-axis position and the y-axis position of the target obstacle, the driving speed and the yaw rate into the following formula to obtain a second heading angle reference value of the target obstacle:
wherein the content of the first and second substances,
Vyrepresenting the y-axis velocity component;
Vxrepresenting the x-axis velocity component;
OBJxrepresenting the x-axis position;
OBJyrepresenting the y-axis position;
e represents the yaw rate;
Vcrepresenting the travel speed.
In a second aspect, the present invention provides an obstacle course angle calculation device, including:
the device comprises an acquisition unit, a processing unit and a control unit, wherein the acquisition unit is used for acquiring at least two first course angle reference values and at least two second course angle reference values of a target obstacle, the first course angle reference values are obtained based on an Oriented Bounding Box (OBB) of the target obstacle, the second course angle reference values are obtained based on preset motion parameters of the target obstacle, and the first course angle reference values and the second course angle reference values are equal in number;
the first determining unit is used for determining a first course angle component and a first weight coefficient corresponding to the first course angle component according to each first course angle reference value, wherein the first weight coefficient is negatively related to the variance of each first course angle reference value and is used for representing the influence degree of the first course angle component on the course angle of the obstacle;
the second determining unit is used for determining a second course angle component and a second weight coefficient corresponding to the second course angle component according to each second course angle reference value, wherein the second weight coefficient is negatively related to the variance of each second course angle reference value and is used for representing the influence degree of the second course angle component on the course angle of the obstacle, and the sum of the second weight coefficient and the first weight coefficient is 1;
and the calculation unit is used for calculating and obtaining the course angle of the target obstacle according to the first course angle component, the first weight coefficient, the second course angle component and the second weight coefficient.
After the first heading angle reference value and the second heading angle reference value are obtained, the first heading angle component and the first weight coefficient corresponding to the first heading angle component are determined according to the first heading angle reference values, the second heading angle component and the second weight coefficient corresponding to the second heading angle component are determined according to the second heading angle reference value, and finally, the heading angle of the target obstacle is obtained through calculation according to the first heading angle component, the first weight coefficient, the second heading angle component and the second weight coefficient.
In the calculation method, the first weight coefficient is negatively correlated with the variance of each first course angle reference value, and represents the influence degree of the first course angle component on the course angle of the obstacle, because the variance can reflect the stability of the first course angle reference value, the smaller the variance is, the smaller the difference between the first course angle reference values is, the higher the credibility is, the larger the value of the corresponding first weight coefficient is, the larger the influence of the first course angle component on the final obstacle course angle calculation result is, and the second weight coefficient can naturally play the same role, the calculation method adjusts the influence degree of the first course angle component and the second course angle component in the final calculation result through the first weight coefficient and the second weight coefficient, so that the more accurate course angle component has more weight, and the advantages of the existing calculation method can be fully played under the condition that the first course angle component and the second course angle component are fused, and the accuracy of the calculation result of the heading angle of the obstacle is improved.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below, and it is obvious that the drawings in the following description are some embodiments of the present invention, and for those skilled in the art, other drawings can be obtained according to these drawings without creative efforts.
Fig. 1 is a flowchart of a method for calculating a heading angle of an obstacle according to an embodiment of the present invention;
fig. 2 is a block diagram of a device for calculating a heading angle of an obstacle according to an embodiment of the present invention.
Detailed Description
In order to make the objects, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are some, but not all, embodiments of the present invention. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
Optionally, referring to fig. 1, fig. 1 is a flowchart of a method for calculating a heading angle of an obstacle, which is provided in an embodiment of the present invention, and is applicable to a controller, such as a vehicle controller, a driving assistance system controller, and a laser radar controller, which can obtain corresponding preset parameters and has data processing capability, and obviously, a server on a network side may be used in some cases; referring to fig. 1, a process of the method for calculating the heading angle of the obstacle according to the embodiment of the present invention may include:
s100, at least two first course angle reference values and at least two second course angle reference values of the target obstacle are obtained.
Optionally, in the embodiment of the present invention, the first course angle reference value is obtained based on the OBB of the target obstacle, and the second course angle reference value is obtained based on a preset motion parameter of the target obstacle. The process of obtaining the first heading angle reference value and the second heading angle reference value is explained below.
And in any detection period, obtaining the three-dimensional point cloud fed back by the laser radar, wherein each point in the point cloud comprises three-dimensional position information and the radar wave reflection intensity of the point. After the three-dimensional point cloud is obtained, the obtained point cloud is divided according to a preset division rule to obtain at least one point cloud cluster. Optionally, a clustering method based on point cloud density, a nearest neighbor method based on kdtree, a k-means method, a deep learning method and the like can be selected for the point cloud partitioning process, and in the prior art, the method for partitioning the obtained three-dimensional point cloud and obtaining at least one point cloud cluster can be selected, and the specific partitioning process of the point cloud cluster is not limited in the embodiment of the invention.
It is conceivable that each point cloud cluster corresponds to one obstacle, any one point cloud cluster is selected as a target point cloud cluster from at least one obtained point cloud cluster, and accordingly, the obstacle corresponding to the target point cloud cluster is the target obstacle. Certainly, in practical application, course angle information of all obstacles in a laser radar detection range needs to be acquired, each point cloud cluster can be used as a target point cloud cluster, correspondingly, the obstacles corresponding to each target point cloud cluster are used as target obstacles, and the course angle calculation method provided by the embodiment of the invention is executed for each target obstacle, so that the course angle of each target obstacle is obtained.
After the target point cloud cluster is determined, the OBB of the target obstacle can be further constructed according to the data information of the target point cloud cluster. Specifically, the construction of the target obstacle OBB may be implemented by using a minimum circumscribed rectangle, a principal component analysis method, a deep learning method, and the like.
The heading angle is a parameter of an OBB model, specifically, the OBB is a rectangle surrounding a target obstacle and comprises long-edge data and wide-edge data, and an included angle between the direction of the long edge and an x axis of a preset coordinate system set based on the designated center position of the vehicle is the heading angle. Therefore, after the target obstacle OBB is obtained, the first heading angle reference value of the target obstacle can be obtained based on the preset coordinate system.
Further, a second course angle reference value of the target obstacle can be calculated by combining preset motion parameters of the target obstacle on the basis of the obtained target obstacle OBB. Specifically, the running speed and the yaw rate of the vehicle are acquired. And according to the OBB of the target obstacle, determining the relative movement speed of the target obstacle in a preset coordinate system set based on the position of the specified center of the vehicle and position parameters in the preset coordinate system, wherein the position parameters of the target obstacle comprise the x-axis position and the y-axis position of the target obstacle in the preset coordinate system. In one possible implementation manner, the origin of the preset coordinate system set based on the designated center position of the vehicle is located at the center of the rear axle of the vehicle, the positive direction of the x-axis is forward, the positive direction of the y-axis is leftward, and the positive direction of the z-axis is upward.
And (2) resolving the relative moving speed of the target obstacle in a preset coordinate system to obtain a corresponding x-axis speed component and a corresponding y-axis speed component, and substituting the parameters of the running speed and the yaw rate of the vehicle, the x-axis position and the y-axis position of the target obstacle in the preset coordinate system and the like into a formula (1) to obtain a second heading angle reference value of the target obstacle through calculation.
Wherein the content of the first and second substances,
Vyrepresenting the y-axis velocity component;
Vxrepresenting the x-axis velocity component;
OBJxrepresenting the x-axis position;
OBJyrepresenting the y-axis position;
e represents the yaw rate;
Vcrepresenting the travel speed.
It should be noted that, in the embodiment of the present invention, the first heading angle reference value and the second heading angle reference value of the target obstacle should be obtained in a combined manner, and the number of the first heading angle reference value and the second heading angle reference value is equal, that is, in each detection period, the first heading angle reference value and the second heading angle reference value are obtained at the same time, and in a certain detection period, only the first heading angle reference value or only the second heading angle reference value is not obtained. Correspondingly, after repeated acquisition of a plurality of detection periods, the first course angle reference value and the second course angle reference value which are equal in quantity and have the corresponding relation can be obtained.
S110, according to each first course angle reference value, determining a first course angle component and a first weight coefficient corresponding to the first course angle component.
Optionally, when the heading angle is calculated based on the OBB of the target obstacle, the accuracy of the calculation result depends on the scanning result of the laser radar on the obstacle, if the laser radar cannot distinguish the head/tail of the obstacle, such as the head/tail of the vehicle, the deviation between the first heading angle reference value calculated based on the OBB and the actual value is 180deg, and in addition, if the laser radar cannot completely scan the obstacle, such as only the tail of the vehicle can be scanned, the first heading angle reference value calculated based on the OBB and the actual value will be 90deg or-90 deg different. Therefore, in order to ensure the accuracy of the target obstacle course angle obtained by fusion in the subsequent step, the first course angle reference value needs to be corrected before the first course angle component and the first weight coefficient are determined.
In the embodiment of the invention, the correction of the first course angle reference value is carried out based on the difference value between the first course angle reference value and the second course angle reference value obtained in the same detection period and the magnitude relation between the first course angle reference value and the second course angle reference value, and the corrected first course angle reference value is defined as a first course angle correction value.
Specifically, for a first course angle reference value and a second course angle reference value obtained in each detection period, a corresponding target correction formula is selected from correction reference conditions given by the formula (2), the first course angle reference value is substituted into the corresponding target correction formula, the obtained correction result is a corresponding first course angle correction value, and all the first course angle reference values are traversed, so that all the first course angle correction values can be obtained.
Wherein the content of the first and second substances,
a _ OBB represents a first heading angle reference value;
a _ mo represents a second heading angle reference value;
a _ OBB _ X represents a first course angle correction value;
m represents a first preset threshold;
n represents a second preset threshold.
According to the formula (2), the correction reference condition set based on the first preset threshold and the second preset threshold is not closed, if A _ OBB-A _ mo is less than or equal to m, the first course angle reference value is correct, correction is not needed, and the first course angle reference value is directly used as the first course angle correction value for subsequent calculation.
Optionally, after obtaining at least two first course angle correction values, obtaining a weight coefficient corresponding to each first course angle correction value, then calculating a product of each first course angle correction value and the corresponding weight coefficient, respectively, to obtain a corresponding first course angle weighted value, further calculating an average value of each first course angle weighted value, and taking the obtained average value as a first course angle component. Specifically, the first heading angle component may be calculated using equation (3).
Wherein N is the number of the first course angle correction values, and N is more than or equal to 1;
wiis the weight coefficient of the ith first course angle correction value, i belongs to [1, N]；
uiIs the ith first course angle correction value, i belongs to [1, N ∈]。
It should be noted that the weight coefficient corresponding to each first course angle correction value is artificially given empirically and is mainly used to balance the influence degree of the historical data on the current data, that is, the weight coefficient corresponding to the first course angle correction value before the acquisition time is smaller, and the weight coefficient corresponding to the first course angle correction value after the acquisition time is larger.
Optionally, in addition to using the average value of the first heading angle weighted values as the first heading angle component, a truncated average value of the first heading angle weighted values may be calculated, that is, a maximum value is removed, a minimum value is removed, and the remaining average value of the first heading angle weighted values is used as the first heading angle component.
Optionally, the first course angle correction values may not be weighted, and an average value or a truncated average value of the first course angle correction values is directly calculated, and the obtained result is used as the first course angle component.
It is conceivable that, under the condition of obtaining the first course angle reference value, if each first course angle reference value is accurate, or the requirement on the accuracy of the calculation result can be reduced, each first course angle reference value is not corrected, the average value or the truncated average value of each first course angle reference value is directly calculated, and the calculation result is used as the first course angle component, which is also optional, and also belongs to the protection scope of the present invention.
It is known that the variance can reflect the stability of the data, and a smaller variance indicates more stable data, whereas a larger variance indicates less stable data. The first weight coefficient provided by the embodiment of the invention is negatively correlated with the variance of each first course angle reference value, namely the greater the variance of each first course angle reference value is, the smaller the first weight coefficient is, and correspondingly, the smaller the influence of the first course angle component on the course angle of the obstacle is; the smaller the variance of each first course angle reference value is, the larger the first weight coefficient is, and correspondingly, the larger the influence of the first course angle component on the course angle of the obstacle is, and the influence degree of the first course angle component on the final calculation result is adjusted by adjusting the first weight coefficient.
It should be noted that, the first heading angle correction value and the first heading angle weighted value of the foregoing contents are obtained based on the first heading angle reference value, and the first weight coefficient is negatively correlated with the variance of the first heading angle reference value, and naturally negatively correlated with the variance of the first heading angle weighted value.
S120, according to the second course angle reference values, a second course angle component and a second weight coefficient corresponding to the second course angle component are determined.
Optionally, the calculation process of the second heading angle component is similar to the calculation process of the first heading angle component, after at least two second heading angle reference values are obtained, a weight coefficient corresponding to each second heading angle reference value is obtained, the product of each second heading angle reference value and the corresponding weight coefficient is respectively calculated, a corresponding second heading angle weighted value is obtained, an average value or a truncated average value of each second heading angle weighted value is calculated, and the obtained result is used as the second heading angle component.
It is contemplated that the weighting factor corresponding to each second heading angle component, similar to the weighting factor corresponding to each first heading angle component, is used to balance the degree of influence of the historical data on the current data, and may be empirically determined. Furthermore, the second heading angle component can also be represented by an average value or a truncated average value of each second heading angle reference value, and the second heading angle component also belongs to the protection scope of the invention on the premise of not exceeding the core thought scope of the invention.
The second weight coefficient is negatively correlated with the variance of each second course angle reference value, and is used for representing the degree of influence of the second course angle component on the course angle of the obstacle, which is not described herein again. In order to ensure the correctness of the finally fused course angle, the sum of the second weight coefficient and the first weight coefficient is 1.
Optionally, based on the above, an embodiment of the present invention provides a method for calculating a first weight coefficient and a second weight coefficient, where the first heading angle component is calculated based on each first heading angle correction value, and the first variance value is calculated according to the first heading angle component and each first heading angle correction value. For the calculation process of the variance, the calculation process can be performed by referring to a known algorithm of the variance, and is not detailed here.
The second course angle component is directly calculated and obtained according to each second course angle reference value, and a second variance value is calculated and obtained according to the second course angle component and each second course angle reference value without using a weight coefficient.
According to the calculation process of the first variance value and the second variance value, in the method, if the variance is calculated by using the first course angle correction value, the weight coefficient adjustment is not adopted for each first course angle correction value, and correspondingly, the first course angle component is directly calculated by using the first course angle correction value; if the first course angle weighted value is selected to calculate the variance, the first course angle component should also be calculated and obtained according to the first course angle weighted value, the consistency of the use data is ensured, and the accuracy of the calculation result is improved. The same is true for the calculation of the second variance, which is not described in detail here.
After the first variance value and the second variance value are obtained, the first variance value and the second variance value are substituted into an equation (4), and a first weight coefficient is obtained:
substituting the first variance value and the second variance value into formula (5) to obtain a second weight coefficient:
wherein the content of the first and second substances,
x1representing a first weight coefficient;
x2represents a second weight coefficient;
pOBBrepresenting a first variance value;
pmorepresenting the second variance value.
As can be seen from the above calculation process, the first weight coefficient and the first variance value are negatively correlated, and correspondingly, the second weight coefficient and the second variance value are also negatively correlated.
S130, calculating to obtain the course angle of the target obstacle according to the first course angle component, the first weight coefficient, the second course angle component and the second weight coefficient.
Calculating the product of the first course angle component and the first weight coefficient to obtain a first product value; and calculating the product of the second course angle component and the second weight coefficient to obtain a second product value. And the sum of the first product value and the second product value is the course angle of the target obstacle.
And calculating the obstacle corresponding to each point cloud cluster by adopting the method, so as to obtain the course angle of each obstacle.
In summary, in the method for calculating the heading angle of the obstacle according to the embodiment of the present invention, the first weight coefficient is negatively correlated with the variance of each first heading angle reference value, and represents the influence degree of the first heading angle component on the heading angle of the obstacle, because the variance can reflect the stability of the first heading angle reference value, the smaller the variance is, it indicates that the difference between the first heading angle reference values is smaller, the higher the reliability is, the larger the value of the corresponding first weight coefficient is, the larger the influence of the first heading angle component on the final obstacle heading angle calculation result is, and the second weight coefficient can naturally play the same role, and the method adjusts the influence degree of the first heading angle component and the second heading angle component in the final calculation result through the first weight coefficient and the second weight coefficient, so that the more accurate heading angle component has a higher proportion, and in the case that the first heading angle component and the second heading angle component are fused, the advantages of the existing calculation method can be fully exerted, and the accuracy of the calculation result of the heading angle of the obstacle is improved.
The following introduces the obstacle course angle calculation device provided in the embodiment of the present invention, and the obstacle course angle calculation device described below may be regarded as a functional module architecture that needs to be set in the central device to implement the obstacle course angle calculation method provided in the embodiment of the present invention; the following description may be cross-referenced with the above.
Optionally, referring to fig. 2, fig. 2 is a block diagram of a structure of an obstacle heading angle calculation device according to an embodiment of the present invention, where the device includes:
the device comprises an acquisition unit 10, a first navigation angle control unit and a second navigation angle control unit, wherein the acquisition unit is used for acquiring at least two first navigation angle reference values and at least two second navigation angle reference values of a target obstacle, the first navigation angle reference values are obtained based on an oriented bounding box OBB of the target obstacle, the second navigation angle reference values are obtained based on preset motion parameters of the target obstacle, and the first navigation angle reference values and the second navigation angle reference values are equal in number;
the first determining unit 20 is configured to determine, according to each first heading angle reference value, a first heading angle component and a first weight coefficient corresponding to the first heading angle component, where the first weight coefficient is negatively correlated with a variance of each first heading angle reference value, and is used to represent a degree of influence of the first heading angle component on the heading angle of the obstacle;
the second determining unit 30 is configured to determine a second heading angle component and a second weight coefficient corresponding to the second heading angle component according to each second heading angle reference value, where the second weight coefficient is negatively correlated with a variance of each second heading angle reference value, and is used to represent an influence degree of the second heading angle component on the heading angle of the obstacle, and a sum of the second weight coefficient and the first weight coefficient is 1;
and the calculating unit 40 is configured to calculate a course angle of the target obstacle according to the first course angle component, the first weight coefficient, the second course angle component, and the second weight coefficient.
Optionally, the first determining unit 20 is configured to, when determining the first heading angle component according to each first heading angle reference value, specifically include:
correcting the first course angle reference value in the detection period based on the difference value and the magnitude relation between the first course angle reference value and the second course angle reference value obtained in the same detection period to obtain a first course angle correction value;
and determining a first course angle component according to each first course angle correction value.
Optionally, the first determining unit 20 is configured to correct the first course angle reference value in the detection period based on a difference and a magnitude relationship between the first course angle reference value and the second course angle reference value obtained in the same detection period, and when obtaining the first course angle correction value, specifically includes:
determining a corresponding correction formula in the following four correction formulas according to the difference value and the magnitude relation of the first course angle reference value and the second course angle reference value obtained in the same detection period to obtain a target correction formula:
wherein the content of the first and second substances,
a _ OBB represents a first heading angle reference value;
a _ mo represents a second heading angle reference value;
a _ OBB _ X represents a first course angle correction value;
m represents a first preset threshold;
n represents a second preset threshold;
and substituting the first course angle reference value into a target correction formula, and taking the obtained correction result as a first course angle correction value.
Optionally, the first determining unit 20 is configured to, when determining the first heading angle component according to each first heading angle correction value, specifically include:
acquiring a weight coefficient corresponding to each first course angle correction value;
respectively calculating the product of each first course angle correction value and the corresponding weight coefficient to obtain a corresponding first course angle weighted value;
and taking the average value or the truncated average value of the weighted values of the first course angles as the first course angle component.
Optionally, the second determining unit 30 is configured to, when determining the second heading angle component according to each second heading angle reference value, specifically include:
acquiring a weight coefficient corresponding to each second course angle reference value;
respectively calculating the product of each second course angle reference value and the corresponding weight coefficient to obtain a corresponding second course angle weighted value;
and taking the average value or the truncated average value of the weighted values of the second heading angles as the second heading angle component.
Optionally, the first determining unit 20 and the second determining unit 30 are configured to, when determining the first weight coefficient and the second weight coefficient, specifically include:
calculating to obtain a first variance value according to the first course angle component and each first course angle correction value;
calculating to obtain a second variance value according to the second course angle component and each second course angle reference value;
substituting the first variance value and the second variance value into the following formula to obtain a first weight coefficient:
substituting the first variance value and the second variance value into the following formula to obtain a second weight coefficient:
wherein the content of the first and second substances,
x1representing a first weight coefficient;
x2represents a second weight coefficient;
pOBBrepresenting a first variance value;
pmorepresenting the second variance value.
Optionally, the obtaining unit 10 is configured to, when obtaining the first heading angle reference value of the target obstacle, specifically include:
in any detection period, point cloud fed back by the laser radar is obtained;
dividing the point cloud according to a preset division rule to obtain at least one point cloud cluster, wherein one point cloud cluster corresponds to one barrier;
determining a target point cloud cluster in at least one point cloud cluster, and obtaining a target obstacle corresponding to the target point cloud cluster;
constructing an OBB of the target obstacle according to the target point cloud cluster;
and obtaining a first course angle reference value based on the OBB of the target obstacle.
Optionally, the obtaining unit 10 is configured to, when obtaining the second heading angle reference value of the target obstacle, specifically include:
acquiring the running speed and the yaw rate of the vehicle;
and determining a second course angle reference value of the target obstacle according to the OBB, the running speed and the yaw rate of the target obstacle.
Optionally, the obtaining unit 10 is configured to, when determining the second heading angle reference value of the target obstacle according to the OBB of the target obstacle, the traveling speed, and the yaw rate, specifically include:
determining the relative movement speed of the target obstacle in a preset coordinate system and the position parameters of the target obstacle according to the OBB of the target obstacle, wherein the preset coordinate system is set based on the designated center position of the vehicle, and the position parameters comprise the x-axis position and the y-axis position of the target obstacle in the preset coordinate system;
substituting the x-axis speed component and the y-axis speed component of the relative moving speed in a preset coordinate system, the x-axis position, the y-axis position, the driving speed and the yaw rate of the target obstacle into the following formula to obtain a second course angle reference value of the target obstacle:
wherein the content of the first and second substances,
Vyrepresenting the y-axis velocity component;
Vxrepresenting the x-axis velocity component;
OBJxrepresenting the x-axis position;
OBJyrepresenting the y-axis position;
e represents a yaw rate;
Vcindicating the speed of travel.
The embodiments in the present description are described in a progressive manner, each embodiment focuses on differences from other embodiments, and the same and similar parts among the embodiments are referred to each other. The device disclosed by the embodiment corresponds to the method disclosed by the embodiment, so that the description is simple, and the relevant points can be referred to the method part for description.
Those of skill would further appreciate that the various illustrative elements and algorithm steps described in connection with the embodiments disclosed herein may be implemented as electronic hardware, computer software, or combinations of both, and that the various illustrative components and steps have been described above generally in terms of their functionality in order to clearly illustrate this interchangeability of hardware and software. Whether such functionality is implemented as hardware or software depends upon the particular application and design constraints imposed on the implementation. Skilled artisans may implement the described functionality in varying ways for each particular application, but such implementation decisions should not be interpreted as causing a departure from the scope of the present invention.
The steps of a method or algorithm described in connection with the embodiments disclosed herein may be embodied directly in hardware, in a software module executed by a processor, or in a combination of the two. A software module may reside in Random Access Memory (RAM), memory, Read Only Memory (ROM), electrically programmable ROM, electrically erasable programmable ROM, registers, hard disk, a removable disk, a CD-ROM, or any other form of storage medium known in the art.
The previous description of the disclosed embodiments is provided to enable any person skilled in the art to make or use the present invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the generic principles defined herein may be applied to other embodiments without departing from the spirit or scope of the invention. Thus, the present invention is not intended to be limited to the embodiments shown herein but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.
Claims (10)
1. A method for calculating a heading angle of an obstacle, comprising:
acquiring at least two first course angle reference values and at least two second course angle reference values of a target obstacle, wherein the first course angle reference values are acquired based on an oriented bounding box OBB of the target obstacle, the second course angle reference values are acquired based on preset motion parameters of the target obstacle, and the first course angle reference values and the second course angle reference values are equal in number;
determining a first course angle component and a first weight coefficient corresponding to the first course angle component according to each first course angle reference value, wherein the first weight coefficient is negatively correlated with the variance of each first course angle reference value and is used for representing the influence degree of the first course angle component on the course angle of the obstacle;
determining a second course angle component and a second weight coefficient corresponding to the second course angle component according to each second course angle reference value, wherein the second weight coefficient is negatively related to the variance of each second course angle reference value and is used for representing the influence degree of the second course angle component on the course angle of the obstacle, and the sum of the second weight coefficient and the first weight coefficient is 1;
and calculating to obtain the course angle of the target obstacle according to the first course angle component, the first weight coefficient, the second course angle component and the second weight coefficient.
2. The method of calculating an obstacle heading angle of claim 1, wherein said determining a first heading angle component based on each of the first heading angle reference values comprises:
correcting the first course angle reference value in the detection period based on the difference value and the magnitude relation between the first course angle reference value and the second course angle reference value obtained in the same detection period to obtain a first course angle correction value;
and determining a first course angle component according to the first course angle correction value.
3. The method of claim 2, wherein the step of correcting the first course angle reference value in the detection period based on the difference and magnitude relationship between the first course angle reference value and the second course angle reference value obtained in the same detection period to obtain a first course angle correction value comprises:
determining a corresponding correction formula in the following four correction formulas according to the difference value and the magnitude relation of the first course angle reference value and the second course angle reference value obtained in the same detection period to obtain a target correction formula:
wherein the content of the first and second substances,
a _ OBB represents a first heading angle reference value;
a _ mo represents a second heading angle reference value;
a _ OBB _ X represents a first course angle correction value;
m represents a first preset threshold;
n represents a second preset threshold;
and substituting the first course angle reference value into a target correction formula, and taking the obtained correction result as a first course angle correction value.
4. The method of calculating an obstacle course angle of claim 2, wherein said determining a first course angle component based on each of said first course angle corrections comprises:
acquiring a weight coefficient corresponding to each first course angle correction value;
respectively calculating the product of each first course angle correction value and the corresponding weight coefficient to obtain a corresponding first course angle weighted value;
and taking the average value or the truncated average value of the weighted values of the first course angle as a first course angle component.
5. The method of calculating an obstacle heading angle of claim 1, wherein said determining a second heading angle component based on each of the second heading angle reference values comprises:
acquiring a weight coefficient corresponding to each second course angle reference value;
respectively calculating the product of each second course angle reference value and the corresponding weight coefficient to obtain a corresponding second course angle weighted value;
and taking the average value or the truncated average value of the weighted values of the second course angle as a second course angle component.
6. The method of claim 2, wherein determining the first and second weighting factors comprises:
calculating to obtain a first variance value according to the first course angle component and each first course angle correction value;
calculating to obtain a second variance value according to the second course angle component and each second course angle reference value;
substituting the first variance value and the second variance value into the following formula to obtain a first weight coefficient:
substituting the first variance value and the second variance value into the following formula to obtain a second weight coefficient:
wherein the content of the first and second substances,
x1representing a first weight coefficient;
x2represents a second weight coefficient;
pOBBrepresenting a first variance value;
pmorepresenting the second variance value.
7. The method of calculating the heading angle of an obstacle according to any one of claims 1-6, wherein the obtaining the first heading angle reference value for the target obstacle comprises:
in any detection period, point cloud fed back by the laser radar is obtained;
dividing the point clouds according to a preset division rule to obtain at least one point cloud cluster, wherein one point cloud cluster corresponds to one obstacle;
determining a target point cloud cluster in at least one point cloud cluster, and obtaining a target obstacle corresponding to the target point cloud cluster;
constructing an OBB of the target obstacle according to the target point cloud cluster;
and obtaining a first course angle reference value based on the OBB of the target obstacle.
8. The method of claim 7, wherein the step of obtaining the second heading angle reference value for the target obstacle comprises:
acquiring the running speed and the yaw rate of the vehicle;
and determining a second course angle reference value of the target obstacle according to the OBB of the target obstacle, the running speed and the yaw rate.
9. The method of calculating an obstacle course angle of claim 8, wherein said determining a second course angle reference value for the target obstacle based on the OBB of the target obstacle, the travel speed, and the yaw rate comprises:
determining the relative movement speed of the target obstacle in a preset coordinate system and the position parameters of the target obstacle according to the OBB of the target obstacle, wherein the preset coordinate system is set based on a specified central position of a vehicle, and the position parameters comprise an x-axis position and a y-axis position of the target obstacle in the preset coordinate system;
substituting the x-axis speed component and the y-axis speed component of the relative movement speed in the preset coordinate system, the x-axis position and the y-axis position of the target obstacle, the driving speed and the yaw rate into the following formula to obtain a second heading angle reference value of the target obstacle:
wherein the content of the first and second substances,
Vyrepresenting the y-axis velocity component;
Vxrepresenting the x-axis velocity component;
OBJxrepresenting the x-axis position;
OBJyrepresenting the y-axis position;
e represents the yaw rate;
Vcrepresenting the travel speed.
10. An obstacle course angle calculation device, comprising:
the device comprises an acquisition unit, a processing unit and a control unit, wherein the acquisition unit is used for acquiring at least two first course angle reference values and at least two second course angle reference values of a target obstacle, the first course angle reference values are obtained based on an Oriented Bounding Box (OBB) of the target obstacle, the second course angle reference values are obtained based on preset motion parameters of the target obstacle, and the first course angle reference values and the second course angle reference values are equal in number;
the first determining unit is used for determining a first course angle component and a first weight coefficient corresponding to the first course angle component according to each first course angle reference value, wherein the first weight coefficient is negatively related to the variance of each first course angle reference value and is used for representing the influence degree of the first course angle component on the course angle of the obstacle;
the second determining unit is used for determining a second course angle component and a second weight coefficient corresponding to the second course angle component according to each second course angle reference value, wherein the second weight coefficient is negatively related to the variance of each second course angle reference value and is used for representing the influence degree of the second course angle component on the course angle of the obstacle, and the sum of the second weight coefficient and the first weight coefficient is 1;
and the calculation unit is used for calculating and obtaining the course angle of the target obstacle according to the first course angle component, the first weight coefficient, the second course angle component and the second weight coefficient.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010238096.0A CN111381249B (en) | 2020-03-30 | 2020-03-30 | Method and device for calculating course angle of obstacle |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010238096.0A CN111381249B (en) | 2020-03-30 | 2020-03-30 | Method and device for calculating course angle of obstacle |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111381249A CN111381249A (en) | 2020-07-07 |
| CN111381249B true CN111381249B (en) | 2021-11-23 |
Family
ID=71215618
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010238096.0A Active CN111381249B (en) | 2020-03-30 | 2020-03-30 | Method and device for calculating course angle of obstacle |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111381249B (en) |
Families Citing this family (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113109783B (en) * | 2021-04-15 | 2022-08-26 | 森思泰克河北科技有限公司 | Course angle acquisition method, device, equipment and storage medium |
| CN113295179B (en) * | 2021-06-04 | 2022-07-05 | 清智汽车科技(苏州)有限公司 | Vehicle course angle correction method and device based on laser sensor |
| CN113609914A (en) * | 2021-07-09 | 2021-11-05 | 北京经纬恒润科技股份有限公司 | Obstacle identification method and device and vehicle control system |
| CN113360795B (en) * | 2021-08-11 | 2021-11-19 | 智道网联科技(北京)有限公司 | Method and device for correcting vehicle course angle, electronic equipment and storage medium |
| US20230150543A1 (en) * | 2021-11-16 | 2023-05-18 | Argo AI, LLC | Systems and methods for estimating cuboid headings based on heading estimations generated using different cuboid defining techniques |
| CN113867370B (en) * | 2021-12-02 | 2022-04-12 | 智道网联科技(北京)有限公司 | Obstacle course angle calculation method and device based on time synchronization |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104590251A (en) * | 2013-10-31 | 2015-05-06 | 现代摩比斯株式会社 | Apparatus and method of generating travel route of vehicle |
| CN105223583A (en) * | 2015-09-10 | 2016-01-06 | 清华大学 | A kind of target vehicle course angle computing method based on three-dimensional laser radar |
| CN106123908A (en) * | 2016-09-08 | 2016-11-16 | 北京京东尚科信息技术有限公司 | Automobile navigation method and system |
| CN106153042A (en) * | 2015-03-31 | 2016-11-23 | 日本电气株式会社 | Course angle acquisition methods and device |
| CN109212531A (en) * | 2017-06-29 | 2019-01-15 | 德尔福技术有限责任公司 | The method for determining target vehicle orientation |
| CN109633676A (en) * | 2018-11-22 | 2019-04-16 | 浙江中车电车有限公司 | A kind of method and system based on the laser radar obstruction detection direction of motion |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US10077047B2 (en) * | 2017-02-10 | 2018-09-18 | Waymo Llc | Using wheel orientation to determine future heading |
-
2020
- 2020-03-30 CN CN202010238096.0A patent/CN111381249B/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104590251A (en) * | 2013-10-31 | 2015-05-06 | 现代摩比斯株式会社 | Apparatus and method of generating travel route of vehicle |
| CN106153042A (en) * | 2015-03-31 | 2016-11-23 | 日本电气株式会社 | Course angle acquisition methods and device |
| CN105223583A (en) * | 2015-09-10 | 2016-01-06 | 清华大学 | A kind of target vehicle course angle computing method based on three-dimensional laser radar |
| CN106123908A (en) * | 2016-09-08 | 2016-11-16 | 北京京东尚科信息技术有限公司 | Automobile navigation method and system |
| CN109212531A (en) * | 2017-06-29 | 2019-01-15 | 德尔福技术有限责任公司 | The method for determining target vehicle orientation |
| CN109633676A (en) * | 2018-11-22 | 2019-04-16 | 浙江中车电车有限公司 | A kind of method and system based on the laser radar obstruction detection direction of motion |
Non-Patent Citations (1)
| Title |
|---|
| 基于激光雷达的智能汽车障碍物检测关键技术研究;刘康;《中国优秀博硕士学位论文全文数据库（硕士）工程科技II辑》;20190715;第50-62页 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111381249A (en) | 2020-07-07 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111381249B (en) | Method and device for calculating course angle of obstacle | |
| CN111309013B (en) | Collision distance determining method and system, vehicle and storage medium | |
| WO2018221453A1 (en) | Output device, control method, program, and storage medium | |
| CN109074490B (en) | Path detection method, related device and computer readable storage medium | |
| CN110674705A (en) | Small-sized obstacle detection method and device based on multi-line laser radar | |
| JP2004538474A (en) | How to determine a model road | |
| US11731649B2 (en) | High precision position estimation method through road shape classification-based map matching and autonomous vehicle thereof | |
| CN114120149B (en) | Oblique photogrammetry building feature point extraction method and device, electronic equipment and medium | |
| CN111563450A (en) | Data processing method, device, equipment and storage medium | |
| CN112415995B (en) | Planning control method based on real-time safety boundary | |
| CN114879659A (en) | Method, device and equipment for identifying retrograde obstacle and storage medium | |
| CN115243932A (en) | Method and device for calibrating camera distance of vehicle and method and device for continuously learning vanishing point estimation model | |
| CN113103957B (en) | Blind area monitoring method and device, electronic equipment and storage medium | |
| CN113609914A (en) | Obstacle identification method and device and vehicle control system | |
| CN117104226A (en) | Cruise target selection method, device, equipment and medium based on lane line | |
| CN111951552A (en) | Method and related device for risk management in automatic driving | |
| CN111547038B (en) | Vehicle control system, equipment and method | |
| CN117002528A (en) | Track planning method, apparatus and storage medium | |
| CN116071714A (en) | Lane departure detection method, system, electronic device, and readable storage medium | |
| CN116385997A (en) | Vehicle-mounted obstacle accurate sensing method, system and storage medium | |
| CN116863325A (en) | Method for multiple target detection and related product | |
| CN111754542A (en) | Target object determination method, tracking method, device, equipment and storage medium thereof | |
| CN116534006B (en) | Vehicle power control method, device and storage medium | |
| CN115112114B (en) | Processing method and device for correcting orientation angle of vehicle around vehicle | |
| Kochenderfer et al. | Robustness of optimized collision avoidance logic to modeling errors |
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 | ||
| CB02 | Change of applicant information | ||
| CB02 | Change of applicant information |
Address after: 4 / F, building 1, No.14 Jiuxianqiao Road, Chaoyang District, Beijing 100020 Applicant after: Beijing Jingwei Hengrun Technology Co., Ltd Address before: 8 / F, block B, No. 11, Anxiang Beili, Chaoyang District, Beijing 100101 Applicant before: Beijing Jingwei HiRain Technologies Co.,Ltd. |
|
| GR01 | Patent grant | ||
| GR01 | Patent grant |