TW202019742A - Lidar detection device for close obstacles and method thereof capable of effectively detecting obstacles and enhancing detection accuracy - Google Patents

Abstract

This invention discloses a lidar detection device for close obstacles and method thereof. The method utilizes four two-dimensional lidars to scan obstacles to obtain the original point cloud data corresponding to the obstacles, which includes relative distances, relative angles, and relative speeds of the obstacles with respect to the vehicle; next, dividing the point cloud data into the point cloud groups corresponding to the obstacles to obtain the boundary length of the obstacle according to the outline of the point cloud group; finally, using Kalman filtering method and extrapolation to predict and track the moving path of dynamic obstacles and transmitting the relative speed and boundary length corresponding to the dynamic obstacle to the automatic driving control device for application. Furthermore, the coordinates of the dynamic obstacle closest to the vehicle may be obtained according to the relative distance, and then transmitting the coordinates to the automatic driving control device for application, so as to effectively detect the obstacles.

Description

Light reaching detection device and method for close obstacles

The invention relates to an obstacle detection technology, and particularly to a light reaching detection device and a method for a close obstacle.

An autonomous vehicle is an intelligent vehicle that senses the road environment through a sensing system, and uses the road, vehicle position, and obstacle information obtained by the sensing to control the driving parameters of the vehicle, thereby enabling the vehicle to safely and reliably drive on the road And reach the intended destination. Autonomous vehicles have applied computer, sensing, communication, information fusion, artificial intelligence and automatic control technologies, and have become the focus of research in countries around the world.

The environmental sensor is an indispensable component in autonomous vehicles, including 77 gigahertz (G Hz) radar, camera, 24 gigahertz (G Hz) radar and three-dimensional lidar (Lidar), two-dimensional lidar ( Lidar), each sensor has its advantages and disadvantages, and can be configured according to the needs of different assisted driving systems. For example, the 77G Hz radar is responsible for long-range detection. The field of view (FOV) is 34 degrees. The blind zone within 2 meters can detect the position 100 meters away from the 77G Hz radar, but there is much noise. FOV is limited and it is difficult to accurately model obstacles. The camera is responsible for long-range detection. The FOV is 90 degrees and the blind zone within 10 meters. It can detect the position 80 meters away from the camera, but it is susceptible to heavy rain, dense fog, strong light and weather. The 24G Hz radar is responsible for mid-range detection. The FOV is 80 degrees. It can detect a position 40 meters away from the 24G Hz radar, but it cannot capture obstacles on the side of the car body (B-pillar), and the detected obstacles The number is limited and the data error is large. Three-dimensional light is responsible for mid-range detection. The FOV is 360 degrees. It can detect the position up to 35 meters from the three-dimensional light, and the blind zone is within 3 meters. Based on the above, currently self-driving cars generally use radar and cameras. However, due to their respective noises, susceptibility to environmental factors, and their inability to be used for short-range detection, the high reliability of 3D light is gradually popularized. However, the data volume of 3D LiDAR is huge, and a higher performance platform is needed to process the corresponding data volume. Furthermore, currently, various types of sensors are generally used to match, to complement each other and limit the use, resulting in The overall cost is more expensive.

Therefore, in view of the above-mentioned problems, the present invention proposes a light reaching detection device and method for close obstacles to solve the problems caused by the conventional knowledge.

The main object of the present invention is to provide a light reaching detection device and method for close obstacles, which uses Kalman filtering and extrapolation to predict and track the moving path of dynamic obstacles to reduce Computing resources, and quickly make up for the errors caused by the two-dimensional light reaching the current unscanned obstacles. In addition, providing the boundary length of dynamic obstacles is more conducive to estimating the collision probability of obstacles than providing only the coordinates of the center point of obstacles.

Another object of the present invention is to provide a light reaching detection device and method for a close obstacle, which uses a two-dimensional light reaching, which has accurate data, wide field of view (FOV) and small blind area. This improves the vehicle's short-range detection capability. In addition, the two-dimensional light reaches the four corners of the vehicle and is located at two different heights. It detects obstacles at close distances and improves the accuracy of sensing.

In order to achieve the above object, the present invention provides a short-range obstacle light detection device, which is provided in a vehicle with at least one obstacle around the vehicle. The short-range obstacle light detection device includes four two-dimensional lights Lidar, a noise filter and a processor. Two of them are located at the front end of the vehicle, and the rest are located at the rear end of the vehicle. All two-dimensional light sources scan the obstacles to obtain the original point cloud data corresponding to the obstacles. The original point cloud data includes the obstacles relative to the vehicle The relative distance, relative angle and relative speed. The noise filter is electrically connected to all two-dimensional light sources, and receives the original point cloud data to filter out the noise of the original point cloud data, thereby generating the filtered point cloud data. The processor is electrically connected to the noise filter and an automatic driving control device provided in the vehicle, and receives and filters point cloud data. The processor divides the filtered point cloud data into at least one point cloud group corresponding to the obstacle with a predetermined length, and obtains the boundary length of the obstacle according to the outline of the point cloud group. When the relative speed changes within a preset period of time, the processor determines that the obstacle is a dynamic obstacle. The processor uses Kalman filtering and extrapolation to predict and track the moving path of the dynamic obstacle, and transmits the corresponding dynamic The relative speed and boundary length of the obstacle are given to the automatic driving control device, and the processor obtains the coordinate of the dynamic obstacle closest to the vehicle according to the relative distance of the dynamic obstacle to transmit the coordinate to the automatic driving control device.

In one embodiment of the present invention, the extrapolation method is Lagrange polynomial interpolation and least squares.

In one embodiment of the present invention, when the relative speed is fixed within a preset time period, the processor determines that the obstacle is a static obstacle, and the processor transmits the relative distance of the static obstacle closest to the vehicle to the automatic driving control device.

In an embodiment of the invention, the distance between the obstacle and the vehicle is less than 20 meters and greater than 0.3 meters.

In one embodiment of the present invention, the two-dimensional light reach at the front end of the vehicle is set at different heights, and the two-dimensional light reach at the rear end of the vehicle is set at different heights.

The invention also provides a light detection method for a close-range obstacle, which detects at least one obstacle around a vehicle. First, use four two-dimensional Lidar to scan obstacles to obtain the original point cloud data corresponding to the obstacle. The original point cloud data includes the relative distance, relative angle and relative speed of the obstacle to the vehicle. Then, the original point cloud data is received to filter the noise of the original point cloud data, thereby generating filtered point cloud data. Next, the filtered point cloud data is received, and the filtered point cloud data is divided into at least one point cloud group corresponding to the obstacle with a predetermined length, and the boundary length of the obstacle is obtained according to the outline of the point cloud group. Finally, determine whether the relative speed changes within a preset period of time: if so, determine the obstacle as a dynamic obstacle, and use Kalman filtering and extrapolation to predict and track the movement path of the dynamic obstacle, and send the corresponding The relative speed and boundary length of the dynamic obstacle are electrically connected to the two-dimensional light automatic driving control device, and the coordinate of the dynamic obstacle closest to the vehicle is obtained according to the relative distance of the dynamic obstacle to transmit the coordinate to the automatic driving control device ; And if not, determine the obstacle as a static obstacle, and transmit the relative distance of the static obstacle closest to the vehicle to the automatic driving control device.

In order to make your reviewer have a better understanding and understanding of the structural features and achieved effects of the present invention, I would like to use the preferred embodiment drawings and detailed descriptions, the explanations are as follows:

The embodiments of the present invention will be further explained in the following with the related drawings. As much as possible, in the drawings and the description, the same reference numerals represent the same or similar components. In the drawings, the shape and thickness may be exaggerated for simplicity and convenience. It can be understood that elements that are not specifically shown in the drawings or described in the specification are in a form known to those of ordinary skill in the technical field. Those of ordinary skill in the art may make various changes and modifications according to the content of the present invention.

Please refer to Figure 1, Figure 2 and Figure 3 below. The light reaching detection device 10 of the short-distance obstacle of the present invention is described below, which is installed in a vehicle 12 with at least one obstacle around the vehicle 12, and the number of obstacles is taken as an example. The distance between each obstacle and the vehicle 12 is less than 20 meters and greater than 0.3 meters. The light detection device 10 includes at least four two-dimensional Lidars 14, a noise filter 16 and a processor 18, wherein the processor 18 includes a calculator and a classifier, and the noise filter 16 may be linear A filter or a non-linear filter. The linear filter is, for example, a Gussan filter, and the non-linear filter is, for example, a median filter. The number of two-dimensional light reaching 14 is four. The two-dimensional light reaches 14 with high data accuracy, wide field of view (FOV) and small blind area, 0.3 meters is the blind area, FOV is 270 degrees, and it can detect the position about 20 meters away from the two-dimensional light 14 Improve the vehicle's short-range detection capability. Therefore, 2D LiDAR can detect the blind area of 3D LiDAR and provide more reliable data than radar. Two of the two-dimensional light reaches 14 are located at the front end of the vehicle 12, and the rest are located at the rear end of the vehicle 12. In order to avoid detection errors when all the two-dimensional light sources 14 are set at the same height, for example, the two-dimensional light sources 14 scan the space between the tires of other vehicles, so that the light detection device 10 assumes that there are no obstacles around Thing. Therefore, the two-dimensional light beams 14 at the front end of the vehicle 12 are respectively set at different heights, and the two-dimensional light beams 14 at the rear end of the vehicle 12 are also set at different heights, respectively, for global detection. The two-dimensional light reaches 14 in four corners of the vehicle. For example, the two-dimensional light beam 14 at the front end of the vehicle is located at 75 cm and 25 cm from the ground, and the two-dimensional light beam 14 at the rear end of the vehicle is located at 75 cm and 25 cm from the ground. All two-dimensional light sources 14 scan all obstacles to obtain the original point cloud data OD corresponding to all obstacles. The original point cloud data OD includes the relative distance, relative angle and relative speed of all obstacles relative to the vehicle 12. The noise filter 16 electrically connects all the two-dimensional light up to 14 and receives the original point cloud data OD to filter out the noise of the original point cloud data OD, thereby generating the filtered point cloud data FD to improve the determination of obstacles The correct rate. The processor 18 receives the filtered point cloud data FD, and divides the filtered point cloud data FD into a plurality of point cloud groups corresponding to all obstacles with a predetermined length, for example, there are five point cloud groups in FIG. 3, The preset length is taken as an example of 50 cm, X represents the distance on the horizontal axis, and Y represents the distance on the vertical axis. Because the distance between the filtered point cloud data FD belonging to the same obstacle will be quite close, usually less than 50 cm, if the distance between the filtered point cloud data FD belonging to different obstacles is usually greater than 50 cm. In other words, the five point cloud groups represent five obstacles around the vehicle 12. The processor 18 obtains the boundary length of each obstacle according to the outline of each point cloud group, thereby distinguishing the size of all obstacles. When the relative speed of the obstacle changes within a preset period of time, the processor 18 determines that the obstacle is a dynamic obstacle, and the processor 18 uses Kalman filtering and extrapolation to predict and track the dynamic obstacle. It moves the path and transmits the relative speed and boundary length of the corresponding dynamic obstacle to the automatic driving control device 20 for use. The processor 18 obtains the coordinate of the dynamic obstacle closest to the vehicle 12 according to the relative distance of the dynamic obstacle, and transmits the coordinate to the automatic driving control device 20 to avoid collision of the vehicle and the obstacle. When the relative speed of the obstacle is fixed within a preset period, the processor 18 determines that the obstacle is a static obstacle, and the processor 18 transmits the relative distance of the static obstacle closest to the vehicle 12 to the automatic driving control device 20 to avoid The vehicle collided with an obstacle.

The invention uses Kalman filtering method and extrapolation method to predict and track the moving path of dynamic obstacles in advance, so as to reduce computing resources, and quickly make up for the errors caused by the unscanned obstacles of the two-dimensional light up to 14. In addition, assuming that the density of the obstacles is the same, the volume of the obstacle is positively related to the mass, and because the volume and length of the obstacle are also positively related, the mass of the obstacle is positively related to the volume; according to Newton's second law of motion, we can obtain It is known that the mass of the obstacle is directly related to the acceleration; from the above, the acceleration of the obstacle is related to the length, so the movement state and length of the obstacle are also related; in other words, because the acceleration of the large obstacle and the small obstacle are not In the same way, this will make the position of the evaluated obstacle different from the actual position; therefore, providing the boundary length and the interval of the dynamic obstacle, compared to the center point coordinates of the obstacle, is more conducive to estimating the probability of collision of the obstacle to avoid The vehicle 12 collides with an obstacle.

The operation method of the light reaching detection device 10 of the short-distance obstacle of the present invention is described below. First, as shown in step S10, all obstacles are scanned using four two-dimensional light beams 14 to obtain the original point cloud data OD corresponding to all obstacles. The original point cloud data OD includes the relative distances of all obstacles to the vehicle 12, Relative angle and relative speed. Next, as shown in step S12, the noise filter 16 receives the original point cloud data OD to filter out the noise of the original point cloud data OD, thereby generating the filtered point cloud data FD. Next, as shown in step S14, the processor 18 receives the filtered point cloud data FD, and divides the filtered point cloud data FD into a plurality of point cloud groups corresponding to all obstacles with a predetermined length, and according to each point cloud The outline of the group obtains the boundary length of each obstacle. Then, as shown in step S16, the processor 18 determines whether the relative speed of the obstacle has changed within a preset period of time, and if so, proceeds to step S18, and if not, proceeds to step S20. In step S18, the processor 18 determines that the obstacle is a dynamic obstacle, and uses Kalman filtering and extrapolation to predict and track the movement path of the dynamic obstacle, and transmits the relative speed and boundary length of the corresponding dynamic obstacle to the power supply The automatic driving control device 20 that is sexually connected to the two-dimensional light reaches 14, and obtains the coordinate of the dynamic obstacle closest to the vehicle 12 according to the relative distance of the dynamic obstacle to transmit the coordinate to the automatic driving control device 20. In step S20, the processor 18 determines that the obstacle is a static obstacle, and transmits the relative distance of the static obstacle closest to the vehicle 12 to the automatic driving control device 20.

The following describes the Kalman filtering method used in the present invention, which is implemented by the following formula (1), formula (2), formula (3), formula (4), and formula (5).

X _{k ｜ (k-1)} = AX _(k-1) +Bu _(k-1) (1)

P _{k ｜ (k-1)} = AP _(k-1) A ^T +Q (2)

K _k =P _{k ｜ (k-1)} H ^T (HP _{k ｜ (k-1)} H ^T +R) ^-1 (3)

X _k = X _{k ｜ (k-1)} +K _k (z _k -HX _{k ｜ (k-1)} ) (4)

P _k = (1-K _k H)P _{k | (k-1)} (5)

k takes an integer greater than or equal to 5 as an example, and formula (1) is a state prediction formula. Time (k-1) is earlier than time k, X _{k | (k-1)} is the position of the obstacle at time k estimated using the position of the obstacle at time (k-1), X _k is the The position of the obstacle at time k, X _(k-1) is the position of the obstacle at time (k-1), A is the state transition matrix, ^AT is the transpose matrix of the state transition matrix, and B is Control matrix, u _(k-1) is the control vector at time (k-1), P _{k | (k-1)} is the covariance at time k estimated by using the covariance matrix at time (k-1) (covariance) matrix, Q is the system noise, P _(k-1) is the covariance matrix at time (k-1), K _k is the Kalman constant at time k, H is the observation model, and H ^T is the observation The transpose of the model, R is the observation error matrix, z _k is the speed of the obstacle at the k-th time, P _k is the covariance matrix at the k-th time, P _{k | (k-1)} is the utilization of the (k-1) The covariance matrix at time predicts the covariance matrix at time k.

The extrapolation methods used in the present invention are, for example, Lagrange polynomial interpolation and least squares. Taking the third-order interpolation method as an example, the position f(t) of the obstacle is shown in formula (6):

×X_(k-2) +

×X_(k-1) +

×X_k (6)f(t)=

×X _(k-2) +

×X _(k-1) +

×X _k (6)

t is time, when t=0, f(0)=X _k . When t=1, f(1) is X _(k+1) . When t=2, f(2) is X _(k+2) . X _(k-2) is the position of the obstacle at time (k-2), X _(k+1) is the position of the obstacle at time (k+1), X _(k+2) is the (k +2) The position of the obstacle at the moment.

Take the least square method as an example. The real position F(t) of the obstacle is shown in formula (7):

F(t)=at ² +bt+c (7)

t is time, and the error value ||F(t)-X _t ||={[F(t)-X _t ] ² } ^1/2 . When t=k, (k-1), (k-2), (k-3) and (k-4), and ||F(t)-X _t || is the minimum, F( t) is the best curve.

Therefore, formula (8), formula (9) and formula (10) are obtained in order.

(8)

(8)

(9)

(9)

(10)

(10)

Since X _k , X _(k-1) , X _(k-2) , X _(k-3) and X _(k-4) are all known, according to formula (10), we can get a, b and c and F(t). When t=(k+1) and (k+2), the future position of the obstacle can be obtained.

In summary, the present invention uses a two-dimensional light source, which has high data accuracy, wide field of view and small blind area, thereby improving the vehicle's close-range detection capability, while using two-dimensional light sources at different heights , To achieve the purpose of global detection.

The above is only a preferred embodiment of the present invention and is not intended to limit the scope of the implementation of the present invention. Therefore, all changes and modifications based on the shape, structure, characteristics and spirit described in the patent application scope of the present invention are cited. , Should be included in the scope of patent application of the present invention.

10: LiDAR detection device 12: vehicle 14: 2D LiDAR 16: noise filter 18: processor 20: automatic driving control device

FIG. 1 is a schematic diagram of the two-dimensional light reaching the vehicle of the present invention. FIG. 2 is a block diagram of an embodiment of a light detection device of the present invention. Figure 3 is a distribution diagram of the filtered point cloud data of the present invention. FIG. 4 is a flowchart of an embodiment of a light detection method of the present invention.

10: LiDAR detection device

14: Two-dimensional light

16: Noise filter

18: processor

20: Automatic driving control device

Claims (14)

A light detection device for short-distance obstacles is provided in a vehicle, and at least one obstacle is around the vehicle. The light detection device for short-distance obstacles includes: four two-dimensional light sensors (Lidar ), where both are located at the front end of the vehicle and the rest are located at the rear end of the vehicle, the two-dimensional light sources scan the obstacles to obtain the original point cloud data corresponding to the at least one obstacle, the original point cloud The data includes the relative distance, relative angle and relative speed of the at least one obstacle with respect to the vehicle; a noise filter is electrically connected to the two-dimensional light sources and receives the original point cloud data to filter out the original point Noise of cloud data, and then generate filtered point cloud data; and a processor, electrically connecting the noise filter and an automatic driving control device provided in the vehicle, and receiving the filtered point cloud data, the The processor divides the filtered point cloud data into at least one point cloud group corresponding to at least one obstacle with a predetermined length, and obtains the boundary length of the at least one obstacle according to the outline of the at least one point cloud group. When the relative speed changes within a preset time period, the processor determines that the at least one obstacle is a dynamic obstacle. The processor uses Kalman filtering and extrapolation to predict and track the moving path of the dynamic obstacle And transmit the relative speed and the boundary length corresponding to the dynamic obstacle to the automatic driving control device, and the processor obtains the coordinate of the dynamic obstacle closest to the vehicle according to the relative distance of the dynamic obstacle, to Transmit the coordinate to the automatic driving control device. The light reaching detection device for a close obstacle according to claim 1, wherein the noise filter is a linear filter or a non-linear filter. The light reaching detection device for a close obstacle according to claim 2, wherein the linear filter is a Gussan filter. The light reaching detection device for a short-distance obstacle according to claim 2, wherein the non-linear filter is a Median filter. The light reaching detection device for short-distance obstacles according to claim 1, wherein when the relative speed is fixed within the preset time period, the processor determines that the at least one obstacle is a static obstacle, and the processor transmits the most The relative distance of the static obstacle approaching the vehicle is given to the automatic driving control device. The light reaching detection device for short-distance obstacles according to claim 1, wherein the extrapolation method is Lagrange polynomial interpolation and least squares. The light reaching detection device for a short-distance obstacle according to claim 1, wherein the distance between the at least one obstacle and the vehicle is less than 20 meters and greater than 0.3 meters. The light reaching detection device of the short-distance obstacle according to claim 1, wherein the two-dimensional light reaching of the front end of the vehicle is set at different heights, and the two-dimensional light reaching of the rear end of the vehicle is set at Different heights. The light reaching detection device of the near obstacle as described in claim 8, wherein the two-dimensional light reaching of the front end of the vehicle are respectively disposed at 75 cm and 25 cm from the ground, and the rear end of the vehicle The two-dimensional light sources are located at 75 cm and 25 cm from the ground. The short-distance obstacle light reaching detection device according to claim 1, wherein the preset length is 50 cm. A light detection method for a short-distance obstacle, which detects at least one obstacle around a vehicle, the light detection method for the short-distance obstacle includes the following steps: using four two-dimensional light (Lidar) Scanning the at least one obstacle to obtain original point cloud data corresponding to the at least one obstacle, the original point cloud data including the relative distance, relative angle and relative speed of the at least one obstacle relative to the vehicle; receiving the original point cloud Data to filter out the noise of the original point cloud data, thereby generating filtered point cloud data; receiving the filtered point cloud data, and dividing the filtered point cloud data into at least one obstacle corresponding to a preset length At least one point cloud group, and obtain the boundary length of the at least one obstacle according to the outline of the at least one point cloud group; determine whether the relative speed changes within a preset period of time: if so, determine the changed at least one obstacle For dynamic obstacles, Kalman filtering and extrapolation are used to predict and track the moving path of the dynamic obstacle, and the relative speed and boundary length corresponding to the dynamic obstacle are transmitted to electrically connect the two Weiguangda's automatic driving control device obtains the coordinate of the dynamic obstacle closest to the vehicle according to the relative distance of the dynamic obstacle to transmit the coordinate to the automatic driving control device; and if not, judges the at least An obstacle is a static obstacle, and the relative distance of the static obstacle closest to the vehicle is transmitted to the automatic driving control device. The method for detecting light reaching a close obstacle according to claim 11, wherein the extrapolation method is Lagrange polynomial interpolation and least squares. The method for detecting light reaching a short-distance obstacle according to claim 11, wherein the distance between the at least one obstacle and the vehicle is less than 20 meters and greater than 0.3 meters. The method for detecting light reaching a close obstacle according to claim 11, wherein the preset length is 50 cm.

Images