CN104597453A - Detection method and device for safety driving area of vehicle corrected by inertial measuring unit - Google Patents

Abstract

The invention provides a method and device for detecting a safe driving area of a vehicle corrected by an inertial measurement device. The method includes: collecting coordinate data of sampling points output by a laser radar fixed on the vehicle; Road surface profile data; collect the angle change value of the laser radar in the pitch direction output by the inertial measurement device linked with the laser radar, and use the angle change value to correct the coordinate data or road surface profile data; process the road surface profile data to obtain multiple The barycenter coordinate data of the space box; calculate the gradient value between the space boxes according to the barycenter coordinate data, and mark multiple space boxes according to the gradient value; perform projection processing on the marked space boxes, and perform midline growth on the projection processing results to generate Vehicle safe driving area. Compared with the prior art, the detection method and device of the present invention can detect the safe running area of the vehicle and improve the accuracy of road surface detection.

Description

Vehicle safe driving area detection method and device corrected by inertial measurement device

technical field

The invention relates to road surface detection technology, in particular to a method and device for detecting a safe driving area of a vehicle corrected by an inertial measurement device.

Background technique

With the continuous improvement of people's safety requirements in the process of driving, it is hoped that the vehicle can actively detect obstacles on the road and identify which areas of the road are safe to drive. This information is very critical for self-driving vehicles. At the same time For manually driven vehicles, it can also play a role in enriching the active and safe driving of vehicles.

In the prior art, sensors such as millimeter-wave radar or image sensors are mainly used to detect the road surface in front of the vehicle. Among them, the detection method using sensors such as millimeter-wave radar has a narrow detection area, and cannot detect small-sized targets, and cannot recognize details such as road slope information. In addition, the detection method using the image sensor shows that the function of the recognition algorithm is not perfect, and the correct recognition rate of the target in the field of view is only 60%-70%. The recognition accuracy rate drops further, and the image quality of the camera is greatly affected by the ambient light, so it cannot work at night. Therefore, the detection effect of the above two methods is very limited.

Contents of the invention

Embodiments of the present invention provide a method and device for detecting a safe driving area of a vehicle corrected by an inertial measurement device, which can detect a safe driving area of a vehicle to improve the accuracy of road surface detection.

In order to achieve the above object, the present invention provides a method for detecting a safe driving area of a vehicle corrected by an inertial measurement device, comprising: a data collection step: collecting coordinate data of sampling points output by a laser radar fixed on the vehicle; a road profile data generation step : Generate road surface profile data in the form of spatial Cartesian coordinates according to the coordinate data; data correction step: collect the angle change value of the lidar in the pitch direction output by the inertial measurement device linked with the lidar, and use the angle The change value corrects the coordinate data or the road surface profile data; the center of gravity coordinate data generation step: process the road surface profile data to obtain the barycenter coordinate data of a plurality of space boxes; the space box marking step: according to the center of gravity The coordinate data calculates the gradient value between the space boxes, and marks the plurality of space boxes according to the gradient value; the step of generating the safe driving area of the vehicle: performs projection processing on the marked space boxes, and performs projection processing on the projection processing results The center line grows to generate the safe driving area of the vehicle.

The present invention also provides a detection device for a safe driving area of a vehicle corrected by an inertial measurement device, comprising: a data collection module for collecting coordinate data of sampling points output by a laser radar fixed on the vehicle; a road profile data generation module for The road surface profile data in the form of spatial Cartesian coordinates is generated according to the coordinate data; the data correction module is used to collect the angle change value of the lidar in the pitch direction output by the inertial measurement device linked with the lidar, and use the The angle change value corrects the coordinate data or the road surface profile data; the barycenter coordinate data generating module is used to process the road surface profile data to obtain barycenter coordinate data of a plurality of space boxes; the space box marking module, It is used to calculate the gradient value between the space boxes according to the center of gravity coordinate data, and mark the plurality of space boxes according to the gradient value; the vehicle safe driving area generation module is used to project the marked space boxes Processing, and the midline growth is performed on the projection processing results to generate the safe driving area of the vehicle.

The vehicle safe driving area detection method and device corrected by the inertial measurement device of the present invention collect coordinate data through laser radar, process the coordinate data to generate road surface profile feature data, and use the collected laser output from the inertial measurement device linked with the laser radar The angle change value of the radar in the pitch direction is corrected for the coordinate data or the road surface profile feature data, and the corrected road surface profile data is processed to obtain multiple space boxes and their center of gravity coordinate data, which are marked based on the center of gravity coordinate data. According to the marked space boxes, the safe driving area of the vehicle can be generated, and at the same time, the measurement error caused by the up and down vibration of the lidar during the driving process of the vehicle is removed, thereby improving the accuracy of road surface detection.

Description of drawings

Fig. 1 is one of schematic diagrams of the working principle of the laser radar according to the embodiment of the present invention;

Fig. 2 is the second schematic diagram of the working principle of the laser radar according to the embodiment of the present invention;

3 is a schematic flowchart of a vehicle safe driving area detection method corrected by an inertial measurement device according to Embodiment 1 of the present invention;

Fig. 4 is a schematic diagram of the conversion principle of the coordinate data of the sampling point output by the lidar;

Fig. 5 is the schematic diagram of the principle of the space box marking step in Fig. 3;

Fig. 6 is a schematic diagram of the principle of the step of generating the safe driving area of the vehicle in Fig. 3;

FIG. 7 is a schematic structural diagram of a vehicle safe driving area detection device corrected by an inertial measurement device according to Embodiment 2 of the present invention.

Label description:

1: lidar; 2: laser beam; 3: sampling point.

Detailed ways

The method and device for detecting the safe driving area of a vehicle corrected by the inertial measurement device according to the embodiments of the present invention will be described in detail below in conjunction with the accompanying drawings, but the examples given are not intended to limit the present invention.

The technical principle of the vehicle safe driving area detection method corrected by the inertial measurement device of the present invention is to use the laser radar to scan the road ahead of the vehicle, collect the coordinate data of the sampling points output by the laser radar, and use the collected coordinate data linked with the laser radar The angular change value of the laser radar in the pitch direction output by the inertial measurement device is used to correct the coordinate data, and process the coordinate data of the sampling point to generate a safe driving area of the vehicle.

Embodiment one

As shown in Figure 1 and Figure 2, it is a schematic diagram of the working principle of the laser radar according to the embodiment of the present invention. In other locations on the vehicle, such as the front bumper of the vehicle, the detection range of this installation will be reduced. When installing, the laser radar 1 is slightly tilted downward, so that it forms a certain angle on the ground, that is, the elevation angle θ of the laser beam, and scans the road ahead to generate the laser beam 2, which is the sampling point that can obtain continuous ranging in the x-axis direction. 3.

Set the laser emission point of the laser radar at the starting point of the vehicle's driving within a data collection section unit (for example, the vehicle has traveled a distance of L meters) as the circle point (0,0,0) of the space Cartesian coordinate system, and the X-axis is the direction parallel to the road surface, the Y axis is the driving direction of the vehicle, and the Z axis is the direction perpendicular to the road surface. For example, the polar coordinates of the sampling point are (r _i , η _i ), and the corresponding spatial rectangular coordinates are (xi _, y _i , z _i ), where i is the serial number of the sampling point of the lidar. For example, when a vehicle is driving along the y-axis, the lidar can scan the road ahead at a fixed scanning frequency, thereby obtaining a large number of discrete sampling points with spatial coordinate information, which can restore the spatial information of the road surface. In practical applications, the lidar can give the polar coordinate data of the sampling point, and can also directly give the converted rectangular coordinate data of the sampling point.

Based on the working principle of Fig. 1 and Fig. 2, and referring to Fig. 3, the vehicle safe driving area detection method corrected by the inertial measurement device in Embodiment 1 of the present invention includes:

Data collection step 301: collecting the coordinate data of the sampling points output by the lidar fixed on the vehicle. Specifically, according to the working principle of the lidar, it can be known that the lidar can output the coordinate data of the sampling point. In addition, depending on the model of the lidar, the maximum range of the polar coordinate η _i of the sampling point may be between 30 and 90 degrees.

Road surface profile data generation step 302: Generate road surface profile data in the form of spatial Cartesian coordinates according to the coordinate data. Specifically, if the coordinate data of the sampling point output by the lidar is in the form of polar coordinates, it is necessary to convert the coordinate data of the sampling point from the form of polar coordinates into the form of rectangular coordinates, and then generate the road surface profile data in the form of rectangular coordinates.

Data correction step 303: collect the angle change value of the laser radar in the pitch direction output by the inertial measurement device linked with the laser radar, and use the angle change value to correct the coordinate data or the road surface profile data. Here, it should be noted that the pitch angle of the laser beam is a fixed value when the vehicle is stationary, and an error occurs due to vibration during the running of the vehicle, so it needs to be corrected. Specifically, the inertial measurement device and the laser radar can be fastened together. Since the vehicle will vibrate during driving, the acceleration of the three coordinate axes of XYZ and the angular acceleration on each coordinate axis can be measured. Through the above data calculation, it can be calculated The angle change σ _j in the pitch direction of the lidar is obtained, and then the coordinate data or the road surface contour data are corrected by using the angle change value. For example, the following equation (1) can be used to calculate the pitch angle θ _i of the laser beam at the i-th sampling point after correction:

_θi = θ- _σj ................................................ ......................................Formula 1)

Among them, i is the serial number of the sampling point of the laser radar, θ is the elevation angle of the laser beam of the laser radar when the vehicle is stationary, σ _j is the angle change value of the laser radar output by the inertial measurement device in the pitch direction, and θ _i is the corrected The pitch angle of the laser beam at the i-th sampling point.

Center-of-gravity coordinate data generation step 304: Process the road surface contour data to obtain the center-of-gravity coordinate data of multiple space boxes.

Space box marking step 305: Calculate the gradient value between the space boxes according to the barycenter coordinate data, and mark multiple space boxes according to the gradient value. In the embodiment of the present invention, the meaning of the space box is to divide the space area into unit boxes of fixed size, which is convenient to put the sampling points in the space area into the corresponding unit boxes, so that the box can be calculated intuitively and simply The barycentric coordinates of , provide the data basis for the calculation of the gradient value in this step.

Step 306 of generating a safe driving area for a vehicle: performing projection processing on the marked space box, and performing midline growth on the projection processing result to generate a safe driving area for a vehicle. It should be noted that, in the embodiment of the present invention, the meaning of midline growth is to search from the midline position of each row in the positive and negative directions along the horizontal axis in the plane area composed of fixed-sized square grids, and according to the search results Obtain the boundary points of the safe driving area of the vehicle.

The vehicle safe driving area detection method corrected by the inertial measurement device of the present invention collects coordinate data through the laser radar, processes the coordinate data to generate road surface profile feature data, and uses the collected laser radar outputted by the inertial measurement device linked with the laser radar. The angle change value in the pitch direction, correct the coordinate data or road surface profile feature data, and process the corrected road surface profile data to obtain multiple space boxes and their center of gravity coordinate data, and mark the multiple spaces based on the center of gravity coordinate data box, according to the marked space boxes, the safe driving area of the vehicle can be generated. Compared with the prior art, the detection method of the safe driving area of the vehicle in the present invention can detect the safe driving area of the vehicle, and at the same time, the laser The measurement error caused by the up and down vibration of the radar during the driving process of the vehicle improves the accuracy of road surface detection.

Further, the coordinate data of the sampling points output by the laser radar can be in the form of polar coordinates, as shown in Figure 4, which is a schematic diagram of the conversion principle of the coordinate data of the sampling points output by the laser radar, combined with the principle shown in Figure 4, the road surface profile The data generating step 302 may specifically include: converting the coordinate data of the sampling point from the polar coordinate form into the rectangular coordinate form, and the following formula (2), formula (3) and formula (4) can be used to calculate the coordinate data of the sampling point under the rectangular coordinate system. X-axis coordinate value x _i , Y-axis coordinate value y _i and Z-axis coordinate value z _i :

x _i ＝r _i sin(η _i )................................... .................................. Equation (2)

${\mathrm{the\; y}}_i=r_i\cos \left(\mathrm{\η}\right)\cos \left({\mathrm{\θ}}_i\right)+\frac{1}{f}\underset{1}{\overset{j}{\mathrm{\Σ}}}{v}_j$ ................................................... ...Formula (3)

z _i ＝r _i cos(η _i )sin(θ _i ).......................... .................................. Equation (4)

Among them, i is the serial number of the sampling point of the laser radar, j is the serial number of the scanning line of the laser radar, r _i is the polar coordinate data of the sampling point of the laser radar, and the circle center of the space Cartesian coordinate system is located in a data acquisition section unit, The laser emission point of the laser radar at the starting point of the vehicle driving, the X axis is the direction parallel to the road surface, the Y axis is the direction of the vehicle driving, and the Z axis is the direction perpendicular to the road surface. The laser emission point is perpendicular to the XY plane to obtain the first Intersection point, the second intersection point obtained by making a perpendicular line from the first intersection point to the scanning line where the sampling point is located, η _i is the angle between the laser beam and the line connecting the laser emission point and the second intersection point, θ _i is the i-th The pitch angle of the laser beam at the sampling point, v _j is the vehicle speed corresponding to the j-th scanning line where the i-th sampling point is located, f is the scanning frequency of the lidar, x _i is the X-axis coordinate of the sampling point, and y _i is The Y-axis coordinate of the sampling point, z _i is the Z-axis coordinate of the sampling point.

Here, it should be noted that a vehicle speed sensor such as a photoelectric encoder can be used to obtain accurate vehicle speed information. Usually, the sampling frequency of the vehicle speed sensor is lower than the scanning frequency of the laser radar. Here, the difference calculation can be performed on the vehicle speed data, so that each scanning line of the laser radar can correspond to a vehicle speed data at the same time. That is, since each scanning speed of the lidar is very fast, but the time interval between each two scanning is relatively long, it can be approximately considered that all sampling points on the scanning line obtain the vehicle speed data at the same time.

Further, the barycenter coordinate data generation step 304 may specifically include: starting from the center of the space Cartesian coordinate system, generating multiple space boxes with a set side length, the multiple space boxes are closely spaced in the three axes of the space Cartesian coordinate system connected; according to the road surface contour data, put all the sampling points into the corresponding space boxes, and remove the space boxes that do not contain any sampling points; identify the serial number of each space box, and calculate the coordinates of the center of gravity of each space box, using the following formula (5), formula (6) and formula (7) realize:

$x_{m,\mathrm{no},q}=\frac{1}{p}\underset{1}{\overset{p}{\mathrm{\Σ}}}{x}_p$ ................................................... ................................. Equation (5)

${\mathrm{the\; y}}_{m,\mathrm{no},q}=\frac{1}{p}\underset{1}{\overset{p}{\mathrm{\Σ}}}{\mathrm{the\; y}}_p$ ................................................... ................................. Equation (6)

$z_{m,\mathrm{no},q}=\frac{1}{p}\underset{1}{\overset{p}{\mathrm{\Σ}}}{z}_p$ ................................................... ................................. Equation (7)

Among them, m is the serial number of the space box in the X-axis direction, n is the serial number of the space box in the Y-axis direction, q is the serial number of the space box in the Z-axis direction, x _{m, n, q} are the X-axis coordinates of the center of gravity of the space box, y _{m, n, q} are the Y-axis coordinates of the center of gravity of the space box, z _{m, n, q} are the Z-axis coordinates of the center of gravity of the space box, p is the number of sampling points contained in the space box, x _p is the space box The X-axis coordinates of the sampling points contained in , y _p is the Y-axis coordinates of the sampling points contained in the space box, and z _p is the Z-axis coordinates of the sampling points contained in the space box. Therefore, after processing the pavement contour data, multiple space boxes and their barycenter coordinate data after the identification sequence are obtained, which provides a data basis for the next space box labeling step.

Further, considering the possible detection error of the lidar and the existence of very small interference objects such as small flying insects that may fly in the detection space, some relatively discrete sampling points should be removed, and the barycenter coordinate data generation step 304 can also be It includes: checking the number of sampling points contained in each space box, and removing the space boxes and the sampling points contained in the space box with the number of sampling points less than a predetermined threshold. Specifically, assuming that the predetermined threshold is 2, all space boxes are checked, and for a space box containing only 1 or 2 sampling points, the space box and the sampling points contained therein may be discarded together.

Further, the space box marking step 305 may specifically include: performing the following processing on each space box:

Find at least one space box adjacent to the space box in the Y-axis positive direction, compare the serial number of the Z-axis direction of at least one space box, and select the space box with the serial number of the largest Z-axis direction;

Calculate the gradient value between the space box and the space box with the serial number in the maximum Z-axis direction. If the gradient value is greater than the first set threshold, mark the space box with the serial number in the maximum Z-axis direction as an unsafe driving area. If the gradient value If it is smaller than the first set threshold, the space box with the largest serial number in the Z-axis direction is marked as a safe driving area.

Specifically, Fig. 5 is a schematic diagram of the principle of the step of marking the space box in Fig. 3. Referring to Fig. 5, assuming that starting from the space box (m, n, q), find the space box adjacent to the positive y-axis of the space box, from Figure 5 shows that there are two space boxes, compare the serial numbers of the two space boxes in the Z-axis direction, and select the space box with the largest serial number in the Z-axis direction, that is, the space box (m,n+1,q+1) , and then calculate the gradient value between these two space boxes, which can be realized by using the following formula (8):

$g_{m,\mathrm{no}+1,q+1}=\frac{|z_{m,\mathrm{no}+1,q+1}-z_{m,\mathrm{no},q}|}{\sqrt{{(x_{m,\mathrm{no},q}-x_{m,\mathrm{no}+1,q+1})}^2+{({\mathrm{the\; y}}_{m,\mathrm{no},q}-{\mathrm{the\; y}}_{m,\mathrm{no}+1,q+1})}^2+{(z_{m,\mathrm{no},q}-z_{m,\mathrm{no}+1,q+1})}^2}}$ .................Formula (8)

Among them, (m,n,q) is the space box, (m,n+1,q+1) is the space box with the serial number of the largest Z-axis direction, g _m,n+1,q+1 is the (m, The gradient between the n,q)th space box and the (m,n+1,q+1)th space box, x _m,n,q is the X of the center of gravity of the (m,n,q)th space box Axis coordinates, y _m,n,q are the Y-axis coordinates of the center of gravity of the (m,n,q)th space box, z _m,n,q are the Z of the center of gravity of the (m,n,q)th space box axis coordinates, is the X-axis coordinate of the center of gravity of the (m,n+1,q+1)th space box, is the Y-axis coordinate of the center of gravity of the (m,n+1,q+1)th space box, is the Z-axis coordinate of the center of gravity of the (m,n+1,q+1)th space box.

After the gradient value between the two space boxes is calculated in the previous step, if the gradient value is greater than the first set threshold (for example, 0.8), the (m, n+1, q+1)th space box Mark it as an unsafe driving area, represented by "l", if the gradient value is less than the first set threshold, mark the (m, n+1, q+1)th space box as a safe driving area, use " 0" to indicate.

Then continue to find the (m+1,n,q)th space box in the positive direction of the x-axis, and use the above method to find that the y-axis where the (m+1,n,q)th space box is located is adjacent in the positive direction and The space box with the largest serial number in the z-axis direction, that is, the (m+1, n+1, q)th space box, also calculates the gradient value between the two space boxes through the above formula (8). Then compare the gradient value with the first set threshold, and mark the (m+1, n+1, q)th space box according to the comparison result. Repeat the above steps until the gradient values in the y-axis direction between all space boxes in the data collection section unit are completely calculated, and those space boxes greater than the first set threshold are marked as 1.

Furthermore, in the space where the space box marked as the unsafe driving area is located, it may be the uneven road surface shape caused by the uneven road surface, or it may be the boundary of obstacles of various shapes on the road surface that are closer to the vehicle, Therefore, the space box marking step 305 may further include: marking space boxes whose Z-axis coordinates are greater than the second set threshold among the center-of-gravity coordinate data of the plurality of space boxes as unsafe driving areas.

Specifically, check the Z-axis coordinates of the barycenter coordinates of all the space boxes in the center-of-gravity coordinate data generation step 304, and mark all the space boxes whose Z-axis coordinates are greater than the second set threshold (for example, 0.3) as unsafe driving areas. In the space where these space boxes are located, there are various obstacles on the road, such as pedestrians, vehicles, trees, railings and other objects with a certain height.

Further, the vehicle safe driving area generation step 306 may specifically include: projecting the marked space box onto the XY plane to generate a plane area composed of a plurality of square grids marked as unsafe driving areas or safe driving areas;

In this plane area, starting from the projection of the center of the space Cartesian coordinate system on the XY plane, the square grids in each row are processed as follows: search from the centerline position of each row along the positive and negative directions of the X axis, Mark the first found square grid marked as an unsafe driving area;

Connect the barycenter coordinates of the space boxes corresponding to all the marked square grids, generate two connecting lines on both sides of the Y axis, and generate the safe driving area of the vehicle from the area between the two connecting lines.

Specifically, FIG. 6 is a schematic diagram of the principle of the step of generating the safe driving area of the vehicle in FIG. 3 . Referring to Figure 6, after the previous step, calculate the gradient value between the space boxes according to the coordinate data of the center of gravity, and mark multiple space boxes according to the gradient value, and obtain multiple space boxes marked with "1" or "0", and then Projecting the marked space box to the XY plane can generate a plane area composed of multiple square grids as shown in Figure 6. In the formed plane, along the positive direction of the Y axis where the zero point is located, from each row The midline position grows towards the positive and negative directions of the X axis. That is to check each space box one by one until it encounters a space box marked "1", and at the same time mark the first space box marked "1" encountered in the two directions of each row, this is For the boundary points of the safe driving area of the vehicle, check the space boxes of each row according to the above method to find all the boundary points. Finally, connect the coordinates of the center of gravity of the space boxes corresponding to all marked square grids, and then generate two connecting lines on both sides of the Y axis, and the area between these two connecting lines is the safe driving area of the vehicle.

In practical applications, the information of the safe driving area of the vehicle can be sent to the central control unit of the self-driving vehicle (or unmanned vehicle) to control the vehicle to move forward safely. For non-autonomous driving vehicles, this information can be used as an important source of information for the vehicle's active safety assisted driving function. When the vehicle approaches an unsafe driving area, it will alert the driver and even actively control the vehicle's braking and turning.

Embodiment two

As shown in Figure 7, it is a schematic structural diagram of a vehicle safe driving area detection device corrected by an inertial measurement device according to Embodiment 2 of the present invention, which includes: a data collection module 701, which is used to collect samples output by a laser radar fixed on a vehicle Point coordinate data; road surface profile data generating module 702, used to generate space Cartesian coordinate form of road surface profile data according to the coordinate data; data correction module 703, used to collect the laser radar outputted by the inertial measurement device linked with the laser radar in the pitch direction The angle change value on the upper surface is used to correct the coordinate data or the road surface contour data; the center of gravity coordinate data generation module 704 is used to process the road surface contour data to obtain the center of gravity coordinate data of a plurality of space boxes; the space box marking module 705, for calculating the gradient value between the space boxes according to the center of gravity coordinate data, and marking a plurality of space boxes according to the gradient value; the vehicle safe driving area generation module 706, for projecting the marked space boxes, and projecting The centerline growth is performed on the processing results to generate the safe driving area of the vehicle.

The vehicle safe driving area detection device calibrated by the inertial measurement device of the present invention collects coordinate data through the laser radar, processes the coordinate data to generate road surface profile feature data, and uses the collected laser radar output from the inertial measurement device linked with the laser radar. The angle change value in the pitch direction, correct the coordinate data or road surface profile feature data, and process the corrected road surface profile data to obtain multiple space boxes and their center of gravity coordinate data, and mark the multiple spaces based on the center of gravity coordinate data box, according to the marked space boxes, the safe driving area of the vehicle can be generated. Compared with the existing technology, the safe driving area of the vehicle can be detected, and at the same time, the vertical vibration generated by the laser radar during the driving process of the vehicle can be removed. measurement error, thereby improving the accuracy of road surface detection.

Further, the barycenter coordinate data generating module 704 may include:

The space box generation unit is used to generate a plurality of space boxes with a set side length with the center of the space Cartesian coordinate system as the starting point, and the multiple space boxes are closely connected on the three axes of the space Cartesian coordinate system;

The sampling point distributing unit is used to put all the sampling points into corresponding space boxes according to the road surface contour data, and remove the space boxes that do not contain any sampling points;

The center of gravity coordinate calculation unit of the space box is used to identify the serial number of each space box and calculate the center of gravity coordinates of each space box, which is realized by the following formula:

${\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; q</span>}}<span\; class="notranslate">\; =</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; p</span>}}\underset{\mathrm{<span\; class="notranslate">\; 1</span>}}{\overset{\mathrm{<span\; class="notranslate">\; p</span>}}{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}}{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; p</span>}}<span\; class="notranslate">\; ,</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; q</span>}}<span\; class="notranslate">\; =</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; p</span>}}\underset{\mathrm{<span\; class="notranslate">\; 1</span>}}{\overset{\mathrm{<span\; class="notranslate">\; p</span>}}{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; p</span>}}<span\; class="notranslate">\; ,</span>{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; q</span>}}<span\; class="notranslate">\; =</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; p</span>}}\underset{\mathrm{<span\; class="notranslate">\; 1</span>}}{\overset{\mathrm{<span\; class="notranslate">\; p</span>}}{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}}{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; p</span>}}$

Among them, m is the serial number of the space box in the X-axis direction, n is the serial number of the space box in the Y-axis direction, q is the serial number of the space box in the Z-axis direction, x _{m, n, q} are the X-axis coordinates of the center of gravity of the space box, y _{m, n, q} are the Y-axis coordinates of the center of gravity of the space box, z _{m, n, q} are the Z-axis coordinates of the center of gravity of the space box, p is the number of sampling points contained in the space box, x _p is the space box The X-axis coordinates of the sampling points contained in , y _p is the Y-axis coordinates of the sampling points contained in the space box, and z _p is the Z-axis coordinates of the sampling points contained in the space box.

The above is only a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Anyone skilled in the art can easily think of changes or substitutions within the technical scope disclosed in the present invention. Should be covered within the protection scope of the present invention. Therefore, the protection scope of the present invention should be determined by the protection scope of the claims.

Claims (10)

1. A vehicle safe driving area detection method corrected by an inertial measurement device, characterized in that, the method comprises: Data acquisition step: collecting the coordinate data of the sampling points output by the lidar fixed on the vehicle; Road profile data generating step: generating road profile data in the form of spatial Cartesian coordinates according to the coordinate data; Data correction step: collecting the angle change value of the laser radar in the pitch direction output by the inertial measurement device linked with the laser radar, and using the angle change value to correct the coordinate data or the road surface profile data; The center of gravity coordinate data generating step: processing the road profile data to obtain the center of gravity coordinate data of a plurality of space boxes; Space box marking step: calculate the gradient value between the space boxes according to the barycenter coordinate data, and mark the multiple space boxes according to the gradient value; The step of generating the safe driving area of the vehicle: performing projection processing on the marked space box, and performing midline growth on the projection processing result to generate the safe driving area of the vehicle. 2. detection method according to claim 1, is characterized in that, the coordinate data of the sampling point that described lidar outputs is polar coordinate form, and in described road profile data generation step, described according to described coordinate data generation The road surface profile data in the form of spatial rectangular coordinates is specifically: The coordinate data of the sampling point is converted from the polar coordinate form into the spatial rectangular coordinate form, and the following formula is used to realize: x _i =r _i sin(η _i ), ${\mathrm{the\; y}}_i=r_i\cos \left({\mathrm{\&eta;}}_i\right)\cos \left({\mathrm{\&theta;}}_i\right)+\frac{1}{f}\underset{1}{\overset{j}{\mathrm{\&Sigma;}}}{v}_j,$ z _i = r _i cos(η _i ) sin(θ _i ), Among them, i is the serial number of the sampling point of the laser radar, j is the serial number of the scanning line of the laser radar, r _i is the polar coordinate data of the sampling point of the laser radar, and the circle center of the space Cartesian coordinate system is located in a data acquisition section unit, The laser emission point of the laser radar at the starting point of the vehicle driving, the X axis is the direction parallel to the road surface, the Y axis is the direction of the vehicle driving, and the Z axis is the direction perpendicular to the road surface. The laser emission point is perpendicular to the XY plane to obtain the first Intersection point, the second intersection point obtained by making a perpendicular line from the first intersection point to the scanning line where the sampling point is located, _ηi is the angle between the laser beam and the line connecting the laser emission point and the second intersection point, _θi is the pitch angle of the laser beam at the i-th sampling point, v _j is the vehicle speed corresponding to the j-th scanning line where the i-th sampling point is located, f is the scanning frequency of the lidar, and x _i is the X-axis coordinate of the sampling point , y _i is the Y-axis coordinate of the sampling point, z _i is the Z-axis coordinate of the sampling point. 3. detection method according to claim 2, is characterized in that, described center of gravity coordinate data generating step is specifically: Taking the center of the space Cartesian coordinate system as the starting point, generating a plurality of space boxes with a set side length, the multiple space boxes are closely connected on the three axes of the space Cartesian coordinate system; Put all the sampling points into the corresponding space box according to the road surface contour data, and remove the space box that does not contain any sampling points; Identify the serial number of each space box, and calculate the coordinates of the center of gravity of each space box, using the following formula to achieve: ${\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; q</span>}}<span\; class="notranslate">\; =</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; p</span>}}\underset{\mathrm{<span\; class="notranslate">\; 1</span>}}{\overset{\mathrm{<span\; class="notranslate">\; p</span>}}{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}}{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; p</span>}}<span\; class="notranslate">\; ,</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; q</span>}}<span\; class="notranslate">\; =</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; p</span>}}\underset{\mathrm{<span\; class="notranslate">\; 1</span>}}{\overset{\mathrm{<span\; class="notranslate">\; p</span>}}{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; p</span>}}<span\; class="notranslate">\; ,</span>{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; q</span>}}<span\; class="notranslate">\; =</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; p</span>}}\underset{\mathrm{<span\; class="notranslate">\; 1</span>}}{\overset{\mathrm{<span\; class="notranslate">\; p</span>}}{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}}{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; p</span>}}<span\; class="notranslate">\; ,</span>$ Among them, m is the serial number of the space box in the X-axis direction, n is the serial number of the space box in the Y-axis direction, q is the serial number of the space box in the Z-axis direction, and x _{m, n, q} are the X-axis of the center of gravity of the space box Coordinates, y _{m, n, q} are the Y-axis coordinates of the center of gravity of the space box, z _{m, n, q} are the Z-axis coordinates of the center of gravity of the space box, p is the sampling point contained in the space box number, x _p is the X-axis coordinate of the sampling point contained in the described space box, y _p is the Y-axis coordinate of the sampling point contained in the described space box, and z _p is the coordinate of the sampling point contained in the described space box Z-axis coordinates. 4. The detection method according to claim 3, wherein the step of generating the barycenter coordinate data further comprises: checking the number of sampling points in each space box, and making the number of sampling points less than a predetermined threshold The space box and the sampling points contained in the space box are removed together. 5. The detection method according to claim 3 or 4, characterized in that, the space box marking step is specifically: Each space box is processed as follows: Find at least one space box adjacent to the space box in the positive direction of the Y axis, compare the serial number of the at least one space box in the Z-axis direction, and select the space box with the largest serial number in the Z-axis direction; Calculate the gradient value between the space box and the space box of the serial number in the maximum Z-axis direction, and if the gradient value is greater than a first set threshold, mark the space box with the serial number in the maximum Z-axis direction as In the non-safe driving area, if the gradient value is smaller than the first set threshold, the space box with the serial number in the maximum Z-axis direction is marked as a safe driving area. 6. The detection method according to claim 5, wherein the step of calculating the gradient value between the space box and the space box of the serial number in the maximum Z-axis direction is realized by the following formula: ${\mathrm{<span\; class="notranslate">\; g</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; q</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; =</span>\frac{<span\; class="notranslate">\; |</span>{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; q</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; q</span>}}<span\; class="notranslate">\; |</span>}{\sqrt{{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; q</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; q</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; q</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; q</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; q</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; q</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}}}<span\; class="notranslate">\; ,</span>$ Wherein, (m, n, q) is the space box, (m, n+1, q+1) is the space box of the serial number of the maximum Z-axis direction, is the gradient between the (m,n,q)th space box and the (m,n+1,q+1)th space box, x _m,n,q is the (m, The X-axis coordinates of the center of gravity of n, q) space boxes, y _{m, n, q} are the Y-axis coordinates of the center of gravity of the (m, n, q) space boxes, and z _{m, n, q} are the The Z-coordinate of the center of gravity of the (m,n,q)th space box, is the X-axis coordinate of the center of gravity of the (m, n+1, q+1)th space box, is the Y-axis coordinate of the center of gravity of the (m, n+1, q+1)th space box, is the Z-axis coordinate of the center of gravity of the (m, n+1, q+1)th space box. 7. The detection method according to claim 6, wherein the space box marking step further comprises: marking the space boxes whose Z-axis coordinates are greater than the second set threshold in the center of gravity coordinate data of the plurality of space boxes It is an unsafe driving area. 8. The detection method according to claim 7, wherein the step of generating the vehicle safe driving area is specifically: Project the marked space box to the XY plane to generate a plane area composed of multiple square grids marked as unsafe driving areas or safe driving areas; In the plane area, starting from the projection of the center of the space Cartesian coordinate system on the XY plane, the square grids in each row are processed as follows: search from the centerline position of each row along the positive and negative directions of the X axis , mark the first found square grid marked as an unsafe driving area; Connect the barycentric coordinates of the space boxes corresponding to all the marked square grids, generate two connecting lines on both sides of the Y axis, and generate a vehicle safe driving area from the area between the connecting lines. 9. A vehicle safe driving area detection device corrected by an inertial measurement device, characterized in that the device comprises: The data collection module is used to collect the coordinate data of the sampling points output by the laser radar fixed on the vehicle; A pavement contour data generating module, configured to generate pavement contour data in the form of spatial Cartesian coordinates according to the coordinate data; A data correction module, configured to collect an angle change value of the laser radar in the pitch direction output by an inertial measurement device linked with the laser radar, and use the angle change value to perform a correction on the coordinate data or the road surface profile data Correction; A barycenter coordinate data generating module, configured to process the road profile data to obtain barycenter coordinate data of a plurality of space boxes; a space box marking module, configured to calculate gradient values between the space boxes according to the barycenter coordinate data, and mark the plurality of space boxes according to the gradient values; The vehicle safe driving area generation module is used for projecting the marked space box, and performing midline growth on the projection processing result to generate the vehicle safe driving area. 10. The detection device according to claim 9, wherein the center-of-gravity coordinate data generating module comprises: A space box generating unit, configured to use the center of the space Cartesian coordinate system as a starting point to generate a plurality of space boxes with a set side length, and the plurality of space boxes are closely connected in the three axial directions of the space Cartesian coordinate system; The sampling point distributing unit is used to put all the sampling points into corresponding space boxes according to the road surface contour data, and remove the space boxes that do not contain any sampling points; The center of gravity coordinate calculation unit of the space box is used to identify the serial number of each space box and calculate the center of gravity coordinates of each space box, which is realized by the following formula: ${\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; q</span>}}<span\; class="notranslate">\; =</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; p</span>}}\underset{\mathrm{<span\; class="notranslate">\; 1</span>}}{\overset{\mathrm{<span\; class="notranslate">\; p</span>}}{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}}{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; p</span>}}<span\; class="notranslate">\; ,</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; q</span>}}<span\; class="notranslate">\; =</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; p</span>}}\underset{\mathrm{<span\; class="notranslate">\; 1</span>}}{\overset{\mathrm{<span\; class="notranslate">\; p</span>}}{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; p</span>}}<span\; class="notranslate">\; ,</span>{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; q</span>}}<span\; class="notranslate">\; =</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; p</span>}}\underset{\mathrm{<span\; class="notranslate">\; 1</span>}}{\overset{\mathrm{<span\; class="notranslate">\; p</span>}}{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}}{\mathrm{<span\; class="notranslate">\; z</span>}}_{\mathrm{<span\; class="notranslate">\; p</span>}}<span\; class="notranslate">\; ,</span>$ Among them, m is the serial number of the space box in the X-axis direction, n is the serial number of the space box in the Y-axis direction, q is the serial number of the space box in the Z-axis direction, and x _{m, n, q} are the X-axis of the center of gravity of the space box Coordinates, y _{m, n, q} are the Y-axis coordinates of the center of gravity of the space box, z _{m, n, q} are the Z-axis coordinates of the center of gravity of the space box, p is the sampling point contained in the space box number, x _p is the X-axis coordinate of the sampling point contained in the described space box, y _p is the Y-axis coordinate of the sampling point contained in the described space box, and z _p is the coordinate of the sampling point contained in the described space box Z-axis coordinates.