CN107632308B - Method for detecting contour of obstacle in front of vehicle based on recursive superposition algorithm - Google Patents
Method for detecting contour of obstacle in front of vehicle based on recursive superposition algorithm Download PDFInfo
- Publication number
- CN107632308B CN107632308B CN201710733511.8A CN201710733511A CN107632308B CN 107632308 B CN107632308 B CN 107632308B CN 201710733511 A CN201710733511 A CN 201710733511A CN 107632308 B CN107632308 B CN 107632308B
- Authority
- CN
- China
- Prior art keywords
- obstacle
- height
- scanning
- probability density
- representing
- 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
Landscapes
- Traffic Control Systems (AREA)
- Optical Radar Systems And Details Thereof (AREA)
Abstract
The invention discloses a method for detecting a vehicle front obstacle contour based on a recursive superposition algorithm, which comprises the following steps: calculating the height of the outline of the obstacle according to the geometric relation between the radar and the ground; matching the historical scanning with the current scanning coordinates; introducing a probability density function by considering the characteristic that radar beams have normal distribution; realizing quasi-continuous estimation of the outline of the obstacle; obtaining the accurate height of the outline of the obstacle in a mode of continuously recursively superposing historical scanning data and current scanning data; calculating the height deviation and the pitch angle deviation through regression; and (4) fusing new and old scans. The measuring method adopted by the invention is simple and feasible, can accurately and efficiently detect the approximate contour height of the obstacle in front of the road, and the overlapped areas generated by new and old scanning are continuously subjected to recursive superposition, so that the influence caused by some interference can be removed, and the information density is greatly enhanced. An accurate vehicle front obstacle profile height can be obtained.
Description
Technical Field
The invention belongs to the technical field of intelligent driving and radars of automobiles, relates to a target identification method by a radar, and particularly relates to a vehicle front obstacle contour detection method based on a recursive superposition algorithm, which is used for solving the problem that the conventional radar cannot quickly and accurately identify the contour of a road obstacle.
Background
The lidar is a commonly used distance measuring sensor, and has the advantages of high resolution, small interference from environmental factors and the like, so that the lidar is widely used in various fields. The laser radar is divided into a single line laser radar, a multi-line laser radar and an area array radar. The single-line laser radar generates one scanning line in each scanning, has the advantages of high ranging speed, small data volume, small volume, light weight and suitability for quick processing, and is most widely used due to high cost performance at present.
Lidar is a type of sensor that uses light waves for distance measurement. Lidar commonly measures the distance of an object using a pulse transit time method. In operation, it emits a laser pulse in the form of a focused beam and measures the transit time between the emitted and received echoes, and since the laser pulse travels at the speed of light, the distance can be determined. The distance from the laser generator to a single reflection point of the target is: d is ct/2, wherein c is the speed of light; t is the time difference from the emission of the laser beam by the ground laser radar to the reception of the echo signal; d is the distance from the laser transmitter to the target reflection point.
The laser beam is well focused by the optical system, so that not only the distance, but also the exact lateral and vertical position of the target relative to the sensor can be determined. By adopting the measuring principle, millimeter-scale distance measurement can be realized, so that the method is very suitable for detecting small changes of road surface unevenness, and the whole ground height profile is constructed.
When the laser radar beam is projected on the surface of dust particles and raindrops, the distance numerical value can be calculated incorrectly. And because on-vehicle radar has a plurality of external interference in the motion state in-process for radar pulse echo is more uncontrollable, consequently must have intelligent analysis algorithm just can reach the practicality.
Currently, more researches are made for measuring the profile of an obstacle in front of a vehicle:
related document 1: application No. 201310063898.2, natura cheng chao, master faith super provides a method and system for estimating a road surface height shape in a road scene using a binocular camera, the method comprising: obtaining a disparity map of a road scene; detecting a road surface region of interest based on the disparity map; determining a plurality of road surface interest points based on the road surface interest areas; and estimating a road surface height shape based on the plurality of road surface points of interest. Because the road scene is very complicated, including pedestrians, vehicles, obstacles and the like, the calculation amount of the algorithm is very large, the off-line processing is generally needed after the data is scanned, and the real-time on-line detection is difficult.
Related document 2: moosmann et al propose an image recognition algorithm with obstacle recognition capability. The algorithm adopts a three-dimensional laser radar to identify and segment the ground and the obstacles. The three-dimensional laser radar has not reached the mass production stage, so the price is high, and the application range of the method is limited.
Related document 3: application No. 201610804686.9, songwei, zhou xiaolong, wu discloses a method for recognizing obstacles using convolutional neural network CNN, performs obstacle recognition based on the bionic eye system using a deep learning algorithm, and provides a method for configuring an interface in the recognition process, thereby enhancing the communication process between the recognition process and the bionic eye system. The method has high recognition rate, but the effective execution of the algorithm must rely on the neural network training of a large number of image model libraries, and once the obstacle information is lacked in the model libraries, the recognition result is greatly influenced.
Disclosure of Invention
The invention aims to provide a vehicle front obstacle contour detection method based on a recursive superposition algorithm, which analyzes laser pulse echo signals for multiple times through multiple continuous scanning recursive matching so as to increase the information density of the signals, accurately obtain the contour height of the vehicle front obstacle, and can be applied to road traffic without limitation.
The purpose of the invention is realized by the following scheme:
a vehicle front obstacle contour detection method based on a recursive superposition algorithm comprises the following steps:
the laser radar is installed at the height position of a headlight at the front part of the vehicle, and can measure a road from the position where a bumper ends, and the light beam of the laser radar is projected on a lane in a relatively inclined manner. The disadvantage caused by the relatively flat scanning angle is relatively slight compared with the measurement length of a few meters in front of a lost vehicle when the laser radar is installed on the vehicle, so that the laser radar is relatively suitable for being installed at the height position of a headlamp to realize the pre-aiming function;
the method comprises the following steps:
calculating the height of an obstacle outline through the geometric relation between a laser radar and the ground, establishing a polar coordinate system, and converting original data of the obstacle outline height acquired by the laser radar into polar coordinates through trigonometric function coordinate transformation;
step two, matching the coordinates of the past scanning data and the current scanning data: converting the obstacle outline height equation from a polar coordinate system representation to a Cartesian coordinate system through trigonometric function coordinate transformation, so as to bring the two scans into the same coordinate system;
step three, introducing a probability density function in consideration of the characteristic that the radar beam has normal distribution: introducing Gaussian normal distribution to obtain a normal distribution probability density function of each measuring point, and representing the real distribution condition of the height of the obstacle profile obtained by the radar measuring point light spot through the probability density function;
step four, realizing quasi-continuous estimation of the obstacle outline: completing the probability density distribution condition of each scanning point by the third step, and performing quasi-continuous estimation on the profile height through a probability density curve; establishing a coordinate system, wherein the abscissa represents the distance from a measuring point scanned by a laser radar beam to a radar, the ordinate represents the height value of the profile of the obstacle, a shift register with equidistant sampling points is introduced on the abscissa, the height value of each scanning is input through a quantized abscissa, and the height value and the probability density of the profile of the obstacle are calculated in a rasterized register according to the distance from the laser radar to the scanning point; taking the sum of n probability density functions of one-time scanning as a unified standard to obtain quasi-continuous estimation of the obstacle profile of each equidistant point of each-time scanning;
step five, obtaining the accurate height of the outline of the obstacle in a mode of continuously recursively superposing the past scanning data and the current scanning data, introducing a correlation coefficient, and evaluating the correlation degree of the influence of the two times of scanning on the height of the real outline of the obstacle;
step six, calculating the height value deviation and the pitch angle deviation through linear regression: obtaining a new relation equation between the current scanning data and the past scanning data through the fifth step, solving through linear regression to obtain the height value deviation and the pitch angle deviation of the profiles of the multiple groups of obstacles, determining the optimal value of the height value deviation and the optimal value of the pitch angle deviation by adopting a least square method, and calculating the height correction value of the new scanning data;
step seven, fusing new scanning data and old scanning data: fusing the current scanning data and the previous scanning data by using the height correction value superposed and superposed in the sixth step to obtain an accurate obstacle contour height value; and meanwhile, updating the height value of the obstacle profile, performing the next recursive superposition, and repeatedly superposing to obtain the accurate vehicle front obstacle profile.
The invention provides a vehicle front obstacle outline detection method based on a recursive superposition algorithm, which only uses a single line laser radar with low price, and has low measurement cost. The invention can be completely independently and efficiently executed without depending on the constraint of any condition such as an external model library and the like, can realize real-time online detection processing, and overcomes a plurality of problems in the field at present.
Drawings
FIG. 1 is a schematic view of the present invention
FIG. 2 is a flow chart of the present invention
FIG. 3 is a diagram of the geometrical relationship between radar and ground for calculating the height of an obstacle profile
FIG. 4 shows a shift register with equidistant sampling points
FIG. 5 is a quasi-continuous estimation of the obstacle profile for each sweep of equidistant points
Detailed Description
The technical scheme of the invention is described in detail in the following with reference to the attached drawings:
a method for detecting the contour of an obstacle in front of a vehicle based on a recursive superposition algorithm is required, a single-line laser radar is required to be installed at the height position of a headlamp at the front of the vehicle, a road can be measured from the position where a bumper ends, and a laser beam is projected on a lane in a relatively inclined mode, and the method comprises the following steps as shown in figure 2:
step one, calculating the height of the outline of the obstacle according to the geometric relation between a laser radar and the ground;
the current obstacle height is calculated using the laser beam of the laser radar installed at the front of the vehicle, as shown in fig. 3.
While the laser radar rotates, the laser beam also provides a distance value of the measuring point i in a fan shape within the measuring angle range. Inclination angle n of pulse light beam emitted by laser radar relative to road0The expression of (a) is:
wherein n iscRepresenting the elevation angle offset of the lidar in the installed position, nLRepresenting the relative pitch angle between the vehicle body and the wheels,
representing the angle of the current lidar measurement beam relative to the sensor housing.
The range of the laser radar measuring angle is 0-90 degrees. The first measurement point of a scan is located at a position rotated approximately 45 deg. downward from the horizontal to the road. By trigonometric function coordinate transformation, the absolute vertical height z from the laser radar to the ground can be calculated by utilizing the installation height of the laser radar and the inclination angle parameter of the laser beam0And the distance x from the measuring point to the sensor in the direction of the x-axis0The calculation formula is as follows:
x0＝d0*cos(n0)
z0＝z-d0*sin(n0)
the original vertical distance z between the lidar and the lane is calculated by:
z＝zcz+zzd-xs*sin(nL)+ys*sin(wL)
the obstacle contour height value z can be deduced0The calculation formula of (a) is as follows:
in the formula, ZczRepresenting the vertical offset Z of the lidar in the installed positioncz；Zzd、nL、wLRespectively representing relative motion vibration, pitch and roll between the vehicle body and the wheels; x is the number ofsAnd ysThe distance between the vehicle gravity center and the laser radar is respectively described in the longitudinal position and the transverse position of the vehicle; d0Representing the distance between the lidar and the measurement point.
Step two, coordinate matching of past scanning data and present scanning data
During the actual running process of the vehicle, the vehicle certainly moves in the longitudinal direction and the transverse direction, and the vehicle body also moves relatively to the road, and the motion parameters in the process can be described as follows: longitudinal running speed v of vehicle bodyxRelative motion vibration z between vehicle body and wheelzdSensor pitch nLSensor swing wL。
It is assumed that the driving state of the vehicle is known, so that two scans can be superimposed. Assuming that the scanning data of two times before and after are known, the equation is converted from polar coordinate representation to Cartesian coordinate system representation through a trigonometric function coordinate conversion relation, and the specific conversion formula is as follows:
past scan data:
now the scan data:
where past scans are denoted by "past", annotated with the subscript "p",
representing the distance from a certain measuring point to the sensor in the direction of the x axis in the past scanning data,
representing the height value of the obstacle profile obtained by past scanning; the scan is now denoted by "now", annotated with the subscript "n",
representing the distance from a certain measuring point to the sensor in the x-axis direction in the present scanning data,
representing the height value of the obstacle profile obtained by the present scan.
Step three, introducing a probability density function in consideration of the characteristic that radar beams have normal distribution
In the equation of the second step, the measured point is considered as a point to be studied, and each distance value measured by the laser radar exactly corresponds to an obstacle profile height value. It is true, however, that each measurement point is actually distributed in the form of a spot, rather than a single point. Within a spot, the height values are distributed with a certain probability. The characteristic that laser radar beams have normal distribution is applied to introduce a normal distribution probability density function, the probability density of a measuring point can be approximated by a continuously distributed function and a Gaussian normal distribution function:
in the above equation, x is a continuous random variable, which in the model can be understood as the horizontal distance between the lidar and the measurement point, and σ is the standard deviation (or variance).
The algorithm realized by the steps above is based on infinitesimal propagation of the measuring points, and the assumption is too idealized to truly describe the height profile of the obstacle. If the two scans have the same distance basis, then the regression analysis will achieve a superposition of the two scans. For this reason, a coordinate system shown in fig. 4 is established, the abscissa representing the distance from the measuring point scanned by the laser radar beam to the radar and the ordinate representing the height value of the obstacle profile. A shift register (which can be understood as an array in an algorithm program) with equidistant sampling points is introduced on the abscissa, and the height value of each scanning is input through a quantized abscissa value, so that the problem of the diffusion of the measuring points is solved.
The shift register with equidistant sampling points has the following advantages: substantially taking into account the planar distribution of the measurement points; the matching of multiple scans has a common distance basis.
Fig. 4 shows an example of the application of the shift register to the measurement point in one scan. Since the measurement point is represented in the form of a light spot in the real case, in which the discrete measurement values may occur with a certain statistical probability, the probability density of the occurrence of the measurement values in the distribution of the measurement points can be given.
After the shift register is introduced, the height value and the probability density of the obstacle outline corresponding to the parameter can be respectively obtained through the parameter of the distance from the laser radar scanning point to the laser radar mounting position.
In the shift register, every distance is Δ x1The equidistant sampling points are divided into horizontal coordinates, namely, the distance between the laser radar scanning point and the radar mounting position is rasterized. As is clear from fig. 4:
the abscissa of the shift register covers the whole measuring range of the laser radar signal, and a shift register with m +1 discrete equidistant sampling points can be obtained according to the grid width and the maximum scanning distance range. For example, if the measurement range is 0-20m and the grid width is 10cm, the shift register has m +1 sampling points, which is 201. In the register, the height value of each measurement point and the probability density distribution are input by the abscissa value x. Table 1 shows the results:
TABLE 1 register with equidistant sampling points for storing scan data
Assuming that a set of scans has k measurement points, the probability density of different measurement spots at each sampling point 0.. m is:
the probability density function can represent the accuracy of the height of the obstacle measured in the light spot, and the larger the probability density peak value is, the more concentrated the probability distribution is, and the higher the measurement accuracy is. The measured data can be continuously processed through the probability density function, and a denser obstacle outline height curve is obtained.
Step four, realizing quasi-continuous estimation of the obstacle outline
Each time a new scan is generated, k height values will be obtained, which are represented in discrete form in distance. In practice, however, there will be a corresponding height value for each position of the shift register where the probability is not zero. Assuming that the scan now consists of k measurement points, the current estimate of the height value can be computed from the m +1 discrete grid points in the shift register, and then from the product of the probability density matrix and the vector of k height values (taking the sum of the k probability density functions of a scan as a uniform criterion):
probability density value at 1 st sampling point is xi0,1、ξ0,2…ξ0,kThe estimated value corresponding to the height value can be calculated by a normalization process:
wherein the weighted sum of the scan data is:
the quasi-continuous estimates of the probability density values and corresponding height values of the 2 nd and 3 … m +1 th sample points can also be obtained by a normalization process.
The probability density value at the m +1 sampling point is xim,1、ξm,2…ξm,kThe estimated value corresponding to the height value can be calculated by a normalization process:
wherein the weighted sum of the scan data is:
according to the algorithm, quasi-continuous estimation of the obstacle profile of each scanning equidistant point can be obtained.
And step five, obtaining the accurate height of the outline of the obstacle in a mode of continuously recursively superposing the past scanning data and the current scanning data.
The signal quality of the obstruction can be greatly improved by using the data of all scans, including the data of the present scan and the past scan, through recursive invocation of the algorithm. This can be achieved by recursively invoking a scan matching algorithm for past scans and present scans at each scan. The recursive superposition algorithm can be briefly described by the following formula:
recursive invocation of the present scan:
recursive invocation of past scanning:
in the formula (I), the compound is shown in the specification,
representing a calculated value of a high value in the present scan data,
representing the sum of probability density functions of the first sampling point in the current scanning data;
represents a calculated value of a high value in past scan data,
the probability density function sum representing the first sampling point in the past scanning data;
representing the distance of the measuring point to the sensor in the x-axis direction in the scan data now,
representing the distance of the measuring point to the sensor in the x-axis direction in the past scan data.
And recursively calling an algorithm function f, namely a fusion process of the new scanning data and the old scanning data in the step seven.
Step six, calculating the height value deviation and the pitch angle deviation through regression
In the recursive addition algorithm, the calculation of the correlation factor must not be considered. When the new scan and the old scan are superimposed by the regression method, the error of the road contour height value must be considered. The height offset or height error in the shift register can be expressed as:
in the formula (I), the compound is shown in the specification,
a height value representing an obstacle profile of a past scan;
a height value representing the currently scanned obstacle profile;
representing a shiftHeight offset or height error in the register.
To superpose and coincide the new scan and the old scan by linear regression, it is necessary to determine how much weight to consider the obstacle contour height value error corresponding to each abscissa value in the shift register. In short, the error of the height value only needs to be considered at the positions with high normalized probability density of the present scan and the past scan. Therefore, regression analysis is considered within the minimum intersection of the probability density distributions of the two scans, since only the overlapping portions of the two scans have a correlation. In view of this consideration, a correlation coefficient R is introduced, which can be calculated from the minimization criterion of the generalized probability density function, and the specific calculation formula is as follows:
in the formula (I), the compound is shown in the specification,
representing the sum of probability density functions in the present scan data and the past scan data, respectively.
In the recursive superposition algorithm, a parameter is added: and the correlation coefficient R also shows that factors such as the light spot plane distribution of the laser measuring points, the height value probability density distribution of the corresponding measuring points and the like are also considered when determining the height value deviation and the pitch angle deviation of the obstacle profile of the current scanning and the past scanning. Between the present scan data and the past scan data, the following new relationship can be derived:
wherein R represents a correlation coefficient;
representing measuring points and excitations in a shift register with equidistant sampling pointsDistance in the direction of the X axis of the optical radar; delta n and delta z respectively represent pitch angle deviation and height value deviation between new scanning data and old scanning data;
this equation is an overdetermined system of equations, similar to Ax ═ b. The above equation can be solved by linear regression. Constructing a generalized inverse matrix A + of the matrix A:
according to the formula, the height value deviation and pitch angle deviation delta n of a plurality of groups of obstacle profiles can be solved, the optimal height value deviation delta z and the optimal pitch angle deviation delta n can be determined by adopting a least square method, and the height correction value of new scanning data can be calculated at the moment:
in the formula, new and old scans are overlapped,
height correction values representing new scan data.
Step seven, fusing new scanning data and old scanning data
Through the implementation of the above steps, the data of the current scanning and the data of the past scanning can be fused at this time by using the correction obtained by the superposition of the previous recursion superposition. After adding the newly scanned data to the saved data of the past scan, the generalized probability density of all previous scans will increase the probability density of the new scan:
in the formula (I), the compound is shown in the specification,
a generalized probability density representing all scans contained in the first sample point;
a generalized probability density sum representing past scan data;
representing the generalized probability density sum of the present scan data.
On the premise that the new probability density is considered, the updated average height value of the road profile can be calculated:
this average height value z0,cy,sumI.e. the resulting accurate obstacle contour height value, in z0,cy,sumAnd replacing the height value of the obstacle profile scanned this time, then performing recursive superposition of the current scanning and the new scanning, calculating the height value of the next scanning, and repeating the recursive superposition to obtain the accurate vehicle front obstacle profile.
The invention has the following advantages:
the new recursive superimposed obstacle contour information processing algorithm considers all boundary conditions applied in real vehicles and can be applied to road traffic without limitation.
The information of a single scan is too incomplete and inaccurate. The present algorithm takes full advantage of the fact that some of the successive scans overlap, to some extent "overlapping" the scans and increasing the information density.
Through the introduction of a probability density function and quasi-continuous estimation, the recursive superposition algorithm can enable radar scanning data to be infinitely approximate to a real value.
To better understand the present invention for those skilled in the art, the present invention is further exemplified by the algorithmic simulation flow of MATLAB.
Based on the fact that MATLAB has strong array computing capability, in order to test the reliability of the algorithm, the MATLAB can be used for building the algorithm of the subject research to perform reliability analysis. The following is to set up a forming algorithm, and assume that two groups of data scanned twice by the laser radar are obtained respectively. Firstly, carrying out coordinate axis transformation on data and realizing the function of an equidistant shift register, and then realizing the matching of two times of scanning by utilizing an interpl function; next, fitting the data obtained by new scanning by using a polyfit and a polyval function to obtain new scanning data after new regression, so as to realize the standardization of a probability density function in the algorithm; then, a correlation coefficient R and a matrix A containing the correlation coefficient are obtained, and x is obtained according to the correlation coefficient R and the matrix A, and the height value deviation and the pitch angle deviation are calculated. And finally fusing the data to obtain a corrected new height value, and finishing the realization of the algorithm.
Claims (8)
1. A method for detecting the contour of an obstacle in front of a vehicle based on a recursive superposition algorithm is characterized in that a single-line laser radar is installed at the height position of a headlamp at the front of the vehicle, a road is measured from the position where a bumper ends, and a laser beam is obliquely projected on a lane, and comprises the following steps:
calculating the height of an obstacle outline through the geometric relation between a laser radar and the ground, establishing a polar coordinate system, and converting original data of the obstacle outline height acquired by the laser radar into polar coordinates through trigonometric function coordinate transformation;
step two, matching the coordinates of the past scanning data and the current scanning data: converting the obstacle outline height equation from a polar coordinate system representation to a Cartesian coordinate system through trigonometric function coordinate transformation, so as to bring the two scans into the same coordinate system;
step three, introducing a probability density function in consideration of the characteristic that the radar beam has normal distribution: introducing Gaussian normal distribution to obtain a normal distribution probability density function of each measuring point, and representing the real distribution condition of the height of the obstacle profile obtained by the radar measuring point light spot through the probability density function;
step four, realizing quasi-continuous estimation of the obstacle outline: completing the probability density distribution condition of each scanning point by the third step, and performing quasi-continuous estimation on the profile height through a probability density curve; establishing a coordinate system, wherein the abscissa represents the distance from a measuring point scanned by a laser radar beam to a radar, the ordinate represents the height value of the profile of the obstacle, a shift register with equidistant sampling points is introduced on the abscissa, the height value of each scanning is input through a quantized abscissa, and the height value and the probability density of the profile of the obstacle are calculated in a rasterized register according to the distance from the laser radar to the scanning point; taking the sum of k probability density functions of one scanning as a unified standard to obtain quasi-continuous estimation of the obstacle profile of each equidistant point of each scanning;
step five, obtaining the accurate height of the outline of the obstacle in a mode of continuously recursively superposing the past scanning data and the current scanning data, introducing a correlation coefficient, and evaluating the correlation degree of the influence of the two times of scanning on the height of the real outline of the obstacle;
step six, calculating the height value deviation and the pitch angle deviation through linear regression: obtaining a new relation equation between the current scanning data and the past scanning data through the fifth step, solving through linear regression to obtain the height value deviation and the pitch angle deviation of the profiles of the multiple groups of obstacles, determining the optimal value of the height value deviation and the optimal value of the pitch angle deviation by adopting a least square method, and calculating the height correction value of the new scanning data;
step seven, fusing new scanning data and old scanning data: fusing the current scanning data and the previous scanning data by using the height correction value superposed and superposed in the sixth step to obtain an accurate obstacle contour height value; and meanwhile, updating the height value of the obstacle profile, performing the next recursive superposition, and repeatedly superposing to obtain the accurate vehicle front obstacle profile.
2. The method for detecting the profile of an obstacle in front of a vehicle based on the recursive superposition algorithm as claimed in claim 1, wherein the step of calculating the height of the obstacle profile through the geometrical relationship between the lidar and the ground comprises the following steps:
inclination angle n of pulse light beam emitted by laser radar relative to road0The expression of (a) is:
wherein n iscRepresenting the elevation angle offset of the lidar in the installed position, nLRepresenting the relative pitch angle between the vehicle body and the wheels,
representing the angle of the current lidar measurement beam relative to the sensor housing;
calculating the height value z of the outline of the obstacle by trigonometric function coordinate transformation and by utilizing the installation height of the laser radar and the inclination angle parameter of the laser beam0And measuring the distance x from the point to the sensor in the direction of the x-axis0The calculation formula is as follows:
x0＝d0*cos(n0)
z0＝z-d0*sin(n0)
the original vertical distance z between the lidar and the lane is calculated by:
z＝zcz+zzd-xs*sin(nL)+ys*sin(wL)
the obstacle contour height value z is derived0The calculation formula of (a) is as follows:
in the formula, ZczRepresenting the vertical offset Z of the lidar in the installed positioncz；Zzd、nL、wLRespectively representing relative motion vibration, pitch and roll between the vehicle body and the wheels; x is the number ofsAnd ysRespectively describing the longitudinal position and the transverse position of the vehicle, the center of gravity of the vehicle andthe distance between the lidar; d0Representing the distance between the lidar and the measurement point.
3. The method for detecting the profile of an obstacle in front of a vehicle based on the recursive superposition algorithm as claimed in claim 1, wherein the second step of matching the coordinates of the past scanning data and the present scanning data comprises the following steps:
assuming that the driving state of the vehicle is known, two scans are thus superimposed;
assuming that the scanning data of the front and the back times are known, the outline height of the obstacle is converted from a polar coordinate representation to a Cartesian coordinate system representation through a trigonometric function coordinate conversion relation, and the specific conversion formula is as follows:
past scan data:
now the scan data:
in the formula (I), the compound is shown in the specification,
representing the distance from a certain measuring point to the sensor in the direction of the x axis in the past scanning data,
representing the height value of the obstacle profile obtained by past scanning;
representing the distance from a certain measuring point to the sensor in the x-axis direction in the present scanning data,
representing the height value of the obstacle profile obtained by the present scan.
4. The method for detecting the profile of the obstacle in front of the vehicle based on the recursive superposition algorithm as claimed in claim 1, wherein the third step introduces the probability density function by considering the characteristic that the radar beam has normal distribution:
the characteristic that laser radar beams have normal distribution is applied to introduce a normal distribution probability density function, the probability density of a measuring point is approximated by a continuous distribution function which is a Gaussian normal distribution function:
wherein x is a continuous random variable representing the horizontal distance between the lidar and the measurement point; σ is the standard deviation;
establishing a coordinate system, wherein the abscissa represents the distance from a measuring point scanned by a laser radar beam to a radar, and the ordinate represents the height value of the outline of the obstacle;
introducing a shift register with equidistant sampling points on an abscissa, wherein a height value of each scanning is input through a quantized abscissa value: respectively solving the height value and the probability density of the obstacle profile corresponding to the parameter by using the parameter of the distance from the laser radar scanning point to the laser radar mounting position;
in the shift register, every distance is Δ x1The equidistant sampling points are divided into horizontal coordinates, namely, the distance between a laser radar scanning point and a radar mounting position is rasterized:
the abscissa of the shift register covers the whole measuring range of the laser radar signal, and a shift register with m +1 discrete equidistant sampling points is obtained according to the grid width and the maximum scanning distance range: i.e. 0, 1
In a register, the height value of the obstacle profile corresponding to each measuring point and the probability density distribution are input through an abscissa value x;
assuming that a set of scans has k measurement points, the probability density of different measurement spots at each sampling point 0.. m is:
the probability density function represents the accuracy of the height of the obstacle measured in the light spot, and quasi-continuous estimation processing is carried out on the measured data through the probability density function to obtain a denser obstacle profile height curve.
5. The method for detecting the obstacle contour in front of the vehicle based on the recursive superposition algorithm as claimed in claim 1, wherein the implementation of the quasi-continuous estimation of the obstacle contour according to the step four comprises the following processes:
probability density value at 1 st sampling point is xi0,1、ξ0,2…ξ0,kThe estimate for the height value is calculated for the normalization process:
wherein the weighted sum of the scan data is:
the probability density values and the quasi-continuous estimation of the corresponding height values of the 2 nd and 3 rd sampling points 3 … are also obtained through standardization processing;
the probability density value at the m +1 sampling point is xim,1、ξm,2…ξm,kThe estimate for the height value is calculated for the normalization process:
wherein the weighted sum of the scan data is:
and obtaining quasi-continuous estimation of the obstacle profile of each scanning sampling point according to the algorithm.
6. The method for detecting the obstacle profile in front of the vehicle based on the recursive superposition algorithm as claimed in claim 1, wherein the step five of obtaining the accurate obstacle profile height by the continuous recursive superposition of the past scanning data and the present scanning data comprises the following processes:
taking the first sampling point as an example, recursively calling a scanning matching algorithm of the past scanning and the present scanning, wherein the recursive superposition algorithm is briefly described by the following formula:
recursive invocation of the scan data now:
recursive invocation of past scan data:
in the formula (I), the compound is shown in the specification,
representing a calculated value of a high value in the present scan data,
representing the sum of probability density functions of the first sampling point in the current scanning data;
represents a calculated value of a high value in past scan data,
the probability density function sum representing the first sampling point in the past scanning data;
representing the distance of the measuring point to the sensor in the x-axis direction in the scan data now,
representing the distance of the measuring point to the sensor in the x-axis direction in the past scan data.
7. The method for detecting the profile of an obstacle ahead of a vehicle based on a recursive superposition algorithm as claimed in claim 1, wherein the sixth step of calculating the altitude value deviation and the pitch angle deviation through regression comprises the following processes:
the height offset in the shift register in step four is represented as:
in the formula (I), the compound is shown in the specification,
an obstacle contour height value representing past scan data;
an obstacle contour height value representing the present scan data;
representing height offset or height error in a shift register；
Introducing a correlation coefficient R:
in the formula (I), the compound is shown in the specification,
representing the sum of probability density functions in the current scan and the past scan respectively;
evaluating the correlation degree of the high influence of the two scans on the real obstacle outline through a correlation coefficient R;
therefore, the following new relation equation is obtained between the current scanning data and the past scanning data:
wherein R represents a correlation coefficient;
representing the distance between a measuring point and the laser radar in the X-axis direction in a shift register with equidistant sampling points; delta n and delta z respectively represent pitch angle deviation and height value deviation between new scanning data and old scanning data;
the above equation is solved by linear regression to construct a generalized inverse matrix a + of matrix a:
and solving the height value deviation and pitch angle deviation of a plurality of groups of obstacle profiles according to the formula, determining the optimal height value deviation delta z and the optimal pitch angle deviation delta n by adopting a least square method, and calculating the height correction value of new scanning data at the moment:
in the formula, new and old scans are overlapped,
height correction values representing new scan data.
8. The method for detecting the contour of an obstacle in front of a vehicle based on a recursive superposition algorithm as claimed in claim 1, wherein the step seven of fusing the old and new scanning data comprises the following processes:
after adding the now scanned data to the saved data of the past scan, the generalized probability density of all previous scans will increase the probability density of the new scan:
Σξ0,sum＝Σξ0,n+Σξ0,p
in the formula, Σ ξ0,sumA generalized probability density representing all scan data contained in the first sample point; Σ xi0,pA generalized probability density sum representing past scan data; Σ xi0,nA generalized probability density sum representing the present scan data;
calculating an updated average height value of the road profile under the premise of considering the new probability density:
this average height value z0,cy,sumI.e. the resulting accurate obstacle contour height value, in z0,cy,sumReplacing the height value of the obstacle profile of the current scanning, then performing the recursive superposition of the current scanning and the new scanning, and calculating the height of the next scanningAnd repeatedly performing recursive superposition to obtain an accurate vehicle front obstacle profile.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710733511.8A CN107632308B (en) | 2017-08-24 | 2017-08-24 | Method for detecting contour of obstacle in front of vehicle based on recursive superposition algorithm |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201710733511.8A CN107632308B (en) | 2017-08-24 | 2017-08-24 | Method for detecting contour of obstacle in front of vehicle based on recursive superposition algorithm |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN107632308A CN107632308A (en) | 2018-01-26 |
| CN107632308B true CN107632308B (en) | 2021-02-05 |
Family
ID=61100143
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201710733511.8A Active CN107632308B (en) | 2017-08-24 | 2017-08-24 | Method for detecting contour of obstacle in front of vehicle based on recursive superposition algorithm |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107632308B (en) |
Families Citing this family (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108445503B (en) * | 2018-03-12 | 2021-09-14 | 吉林大学 | Unmanned path planning algorithm based on fusion of laser radar and high-precision map |
| CN109447943B (en) * | 2018-09-21 | 2020-08-14 | 中国科学院深圳先进技术研究院 | Target detection method, system and terminal equipment |
| CN109633676A (en) * | 2018-11-22 | 2019-04-16 | 浙江中车电车有限公司 | A kind of method and system based on the laser radar obstruction detection direction of motion |
| CN109633685A (en) * | 2018-11-22 | 2019-04-16 | 浙江中车电车有限公司 | A kind of method and system based on laser radar obstruction detection state |
| CN111845730B (en) | 2019-04-28 | 2022-02-18 | 郑州宇通客车股份有限公司 | Vehicle control system and vehicle based on barrier height |
| CN113296117B (en) * | 2020-04-22 | 2023-08-08 | 追觅创新科技(苏州)有限公司 | Obstacle recognition method, obstacle recognition device and storage medium |
| CN111505623B (en) * | 2020-04-24 | 2023-04-18 | 中南大学 | Method and system for detecting obstacle in driving process of unmanned vehicle and vehicle |
| CN112947454A (en) * | 2021-02-25 | 2021-06-11 | 浙江理工大学 | Fire fighting evaluation method, device, equipment and storage medium for warehouse |
| CN113777616B (en) * | 2021-07-27 | 2024-06-18 | 武汉市异方体科技有限公司 | Distance measuring method for moving vehicle |
| CN114485855A (en) * | 2022-01-26 | 2022-05-13 | 吉林大学 | Vehicle front surface ponding depth detection system based on self-vehicle sensor |
| CN114994638B (en) * | 2022-08-04 | 2022-11-29 | 之江实验室 | Automatic driving automobile obstacle identification method based on elliptic envelope curve set |
Family Cites Families (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP5470886B2 (en) * | 2009-02-12 | 2014-04-16 | トヨタ自動車株式会社 | Object detection device |
| US9118816B2 (en) * | 2011-12-06 | 2015-08-25 | Mobileye Vision Technologies Ltd. | Road vertical contour detection |
| CN102721397A (en) * | 2012-06-07 | 2012-10-10 | 江苏科技大学 | Method for extracting road surface characteristic parameters based on modern time series of vertical dynamic load |
| CN103197323B (en) * | 2013-04-17 | 2014-11-26 | 清华大学 | Scanning data matching method and device for laser distance measuring machine |
| CN103669183B (en) * | 2013-12-02 | 2016-05-25 | 黑龙江科技大学 | The time series models of surface evenness |
| CN104233935B (en) * | 2014-08-28 | 2016-05-11 | 吉林大学 | A kind of pavement quality grade discrimination method based on profile of road information |
-
2017
- 2017-08-24 CN CN201710733511.8A patent/CN107632308B/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN107632308A (en) | 2018-01-26 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107632308B (en) | Method for detecting contour of obstacle in front of vehicle based on recursive superposition algorithm | |
| CN109212531B (en) | Method for determining the orientation of a target vehicle | |
| CN1940591B (en) | System and method of target tracking using sensor fusion | |
| CN109085570A (en) | Automobile detecting following algorithm based on data fusion | |
| CN110758379B (en) | Method and device for detecting inclined parking space and automatic parking method and system | |
| Thormann et al. | Extended target tracking using Gaussian processes with high-resolution automotive radar | |
| CN105404844A (en) | Road boundary detection method based on multi-line laser radar | |
| CN112363167A (en) | Extended target tracking method based on fusion of millimeter wave radar and monocular camera | |
| CN107796373B (en) | Distance measurement method based on monocular vision of front vehicle driven by lane plane geometric model | |
| Park et al. | Radar localization and mapping for indoor disaster environments via multi-modal registration to prior LiDAR map | |
| US10509980B2 (en) | Method to provide a vehicle environment contour polyline from detection data | |
| CN112781599B (en) | Method for determining the position of a vehicle | |
| CN110568433A (en) | High-altitude parabolic detection method based on millimeter wave radar | |
| CN112083441A (en) | Obstacle detection method and system based on deep fusion of laser radar and millimeter wave radar | |
| CN112198503A (en) | Target track prediction optimization method and device and radar system | |
| CN110850859B (en) | Robot and obstacle avoidance method and obstacle avoidance system thereof | |
| CN113850102A (en) | Vehicle-mounted vision detection method and system based on millimeter wave radar assistance | |
| CN112379393B (en) | Train collision early warning method and device | |
| CN115079143B (en) | Multi-radar external parameter quick calibration method and device for double-bridge steering mine card | |
| CN112710339A (en) | Method and apparatus for calibrating vehicle sensors | |
| JPH07120555A (en) | Environment recognition device for vehicle | |
| CN114821526A (en) | Obstacle three-dimensional frame detection method based on 4D millimeter wave radar point cloud | |
| CN114137562B (en) | Multi-target tracking method based on improved global nearest neighbor | |
| CN113296120B (en) | Obstacle detection method and terminal | |
| US20230094836A1 (en) | Method for Detecting Moving Objects in the Surroundings of a Vehicle, and Motor Vehicle |
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 |