Obstacle Detection Method Based on 3D Point Cloud Forward Projection Map using Fractional Differentials

Abstract

This invention discloses a method for obstacle detection based on fractional derivatives in a 3D point cloud forward projection map. The method first projects point cloud data collected by a LiDAR onto a forward view based on horizontal and vertical offset angles. The pixel value of each pixel in the forward view is determined by the maximum distance of all point clouds falling into that pixel. Then, the fractional derivative solution process is transformed into a one-dimensional convolution in the y-direction for each pixel. Finally, the result of the fractional derivative is used to determine whether the point cloud within that pixel is an obstacle. This invention detects environmental obstacles during robot movement by solving for fractional derivatives, reducing the possibility of missed detections and improving the safety of robot movement.

Description

Technical Field

[0001] This invention belongs to the field of three-dimensional point cloud processing technology. Specifically, it is a method for obstacle detection of three-dimensional point cloud forward projection map based on fractional derivative. Background Technology

[0002] Obstacle detection is a crucial aspect of environmental perception for autonomous vehicles, significantly impacting their safe driving and optimal route selection. The quality of obstacle detection directly affects the safety of autonomous vehicles. With the widespread application of high-real-time, high-accuracy sensors in robotics and the rapid advancements in computer performance, obstacle detection has become widely used in robotics. In recent years, LiDAR has gradually replaced vision cameras as the primary sensor for obstacle detection in mobile robots. LiDAR is divided into traditional repetitive scanning LiDAR and the currently more common non-repetitive scanning LiDAR. Repetitive scanning LiDAR repeatedly scans the same locations with a fixed number of lines, often resulting in insufficient information in the scene. Currently, Lanvo has launched a non-repetitive scanning LiDAR that uses prism rotation to scan different locations in each frame, enriching the information in the scene.

[0003] Obstacle detection methods for point cloud data mainly include traditional point cloud obstacle detection and deep learning-based point cloud obstacle detection. Traditional methods typically perform plane fitting on all points in the scene. Each time, three points are randomly selected from the scene, forming a plane. The distances from all other points to this plane are then calculated. Points with a distance greater than a threshold τ are identified as outside points; otherwise, they are identified as inside points. This process is repeated multiple times, and the plane corresponding to the point with the most inside points is saved and designated as the ground plane in the current scene. Points farther from the ground plane can then be identified as obstacles. This ground plane fitting-based obstacle detection algorithm usually requires numerous iterations to find the best-fitting ground parameters, which is time-consuming and also struggles to distinguish between obstacles such as slopes.

[0004] Deep learning-based methods typically divide point clouds into voxel blocks, extract features from each voxel block, and then concatenate the extracted voxel features to reconstruct a feature map. This feature map is then fed into a convolutional neural network (CNN), and the network's output determines the region where the obstacle is located. CNN-based methods usually employ supervised learning for training, which requires a large amount of manually labeled training data. Furthermore, deep learning-based methods have high hardware requirements, and edge computing devices installed on robots are generally insufficient to meet these demands. Summary of the Invention

[0005] The purpose of this invention is to provide a method for obstacle detection based on fractional derivatives of three-dimensional point cloud forward projection maps, enabling robots to move safely in the environment based on the obstacle detection results.

[0006] The technical solution for achieving the objective of this invention is: a method for obstacle detection based on fractional derivatives in a three-dimensional point cloud forward projection map, comprising the following steps:

[0007] Step 1: Based on the acquisition resolution of the LiDAR, set the horizontal and vertical angular resolution represented by each pixel in the front view, as well as the horizontal and vertical ranges of the selected point cloud.

[0008] Step 2: Filter the point cloud within the selected area. Project the filtered point cloud onto the corresponding pixels in the front view based on their horizontal and vertical offsets. Use the normalized distance from the point cloud to the LiDAR as the pixel value for that point. (If the front view...) Figure 1 If a pixel falls into multiple point cloud data, the pixel value of the point is selected as the minimum distance among all point clouds.

[0009] Step 3: Calculate the fractional derivative for each pixel in the front view. This process is achieved by solving the one-dimensional convolution in the y-direction for each pixel.

[0010] Step 4: Determine the fractional derivative value of each pixel. If it is greater than a threshold, then all point clouds corresponding to that pixel can be identified as obstacle point clouds.

[0011] Furthermore, the specific method for projecting the filtered point cloud data onto the front view in the second step is as follows:

[0012] 1) Set the horizontal resolution Δθ and vertical resolution of the front view according to the acquisition resolution of the lidar. The angular resolution of the front view is set to be greater than the acquisition resolution of the lidar;

[0013] 2) The lidar acquires a frame of raw point cloud data at time t, which is represented by a set Z. t express

[0014]

[0015] Where i is the index value of the point cloud, and N represents the number of points in a frame of the point cloud, (x i y i , z i () represents the coordinates of the point cloud at index i in the Cartesian coordinate system; the coordinates of the point cloud in the Cartesian coordinate system are projected to the spherical coordinate system according to the following formula:

[0016]

[0017]

[0018]

[0019] Where, d i θ represents the measured distance from the point cloud at index i to the lidar. i This represents the horizontal rotation angle of the point cloud at index i relative to the origin. This represents the vertical rotation angle of the point cloud at index i relative to the origin; the projected point cloud is represented by a set. express:

[0020]

[0021] 3) Filter out point clouds that do not belong to the preset range, and calculate the front view coordinate index corresponding to each point based on the filtered point cloud data and the preset front view angular resolution:

[0022] P i_x =floor(θ) i ÷Δθ)

[0023]

[0024] Among them, P i_x P i_y Let represent the x-coordinate index and y-coordinate index of the point cloud corresponding to index i in the front view, respectively, with floor indicating rounding down; after determining the front view pixels corresponding to each point, the distance value d of each point is... i_ralue Represented by the minimum point cloud distance falling within that pixel:

[0025]

[0026] Furthermore, the front view properties in the second step are shown in the table below:

[0027]

[0028] Furthermore, the method for calculating the fractional derivative of each pixel in the front view using one-dimensional convolution in the third step is as follows:

[0029] 1) Construct a two-dimensional matrix of type Eigen with a size of 3 rows (w×h) columns. Use cv::Mat to read and save a front view. Iterate through each point in the front view and save the pixel value of each point and the pixel values ​​of the points in the same position in the first two rows, a total of 3 values, into the two-dimensional matrix of type Eigen. If the first two rows are empty, fill them with 0.

[0030] 2) The fractional derivative of the image in the y-direction is expressed by the following formula:

[0031]

[0032] Where x and y are the index values ​​of the horizontal and vertical coordinates of the image, respectively, f(x, y) is the pixel value of the pixel with index x and y, and v represents the order of the fractional derivative, ranging from 0 to 1; construct a one-dimensional matrix of type Eigen, with a size of 1 row and 3 columns, and the matrix is ​​represented by the following formula:

[0033]

[0034] 3) Multiply the 3-row (w×h) matrix obtained in 1) with the 1-row, 3-column matrix obtained in 2), using the following formula:

[0035]

[0036] Finally, a matrix with 1 row and (w×h) columns is obtained, which is saved into an image of size w×h, representing the fractional derivative value of each pixel.

[0037] Furthermore, in the fourth step, the specific method for determining obstacles based on the fractional derivative is as follows:

[0038] 1) Solve for the angle σ of the right triangle with the difference between the opposite sides and l as the adjacent side, where l represents the angle with vertical resolution. Let be the included angle, and the arc length of the sector with the distance within the point cloud of the currently calculated pixel as its radius. The arc length can be approximated as a straight line. The specific formula for solving this problem is as follows:

[0039]

[0040] 2) When the value of the angle σ is negative and close to -90°, it means that the line connecting the point cloud in this pixel and the point cloud in the neighboring pixel is parallel to the horizontal road surface; when the angle σ is positive, it means that the line connecting the point cloud in this pixel and the point cloud in the neighboring pixel is parallel to a vertical plane; a threshold τ is set according to the robot's obstacle crossing ability. When the angle σ is less than τ, it is determined to be a non-obstacle, and when the angle σ is greater than τ, it is determined to be an obstacle.

[0041] Compared with the prior art, the present invention has the following significant advantages: The present invention uses fast convolution and matrix operation methods to solve fractional derivatives, which greatly improves the calculation speed of fractional derivatives; the use of fractional derivatives can effectively solve the problem of not being able to find derivatives due to the lack of neighboring pixels when using integer derivatives, and fractional derivatives have better robustness in dealing with the ranging error of lidar compared with integer derivatives. Attached Figure Description

[0042] Figure 1This is a schematic diagram of the original point cloud.

[0043] Figure 2 This is a schematic diagram of the variables in an obstacle scene.

[0044] Figure 3 This is a schematic diagram of the variables in a flat road scene.

[0045] Figure 4 This is the result of the point cloud projection front view.

[0046] Figure 5 This is the result of obstacle detection.

[0047] Figure 6 These are real-world camera photos.

[0048] Figure 7 This is the overall flowchart of the present invention. Detailed Implementation

[0049] The present invention will be further described below with reference to the accompanying drawings and embodiments.

[0050] The present invention provides a method for obstacle detection based on fractional derivatives in a three-dimensional point cloud forward projection map, the method comprising the following steps:

[0051] Step 1: Set the horizontal and vertical angular resolution represented by each pixel in the front view according to the LiDAR's acquisition resolution, and select the horizontal and vertical range of the point cloud according to the LiDAR's acquisition range. This includes the following steps:

[0052] 1) Download the corresponding LiDAR driver file and instruction manual. Based on the acquisition resolution specified in the instruction manual, determine the horizontal angular resolution Δθ and vertical resolution represented by each pixel in the front view. The chosen Δθ and We should try our best to ensure that there is a point cloud in every pixel.

[0053] 2) Based on the collected point cloud data and the horizontal and vertical fields of view specified in the instruction manual, determine the horizontal and vertical ranges θ for selecting point clouds. The chosen θ and It must not exceed the acquisition range of the lidar.

[0054] Step 2: Project the acquired raw point cloud onto the corresponding pixels in the front view according to its horizontal and vertical offsets, and use the normalized distance from the point cloud to the LiDAR as the pixel value of that point. (If the front view...) Figure 1 If multiple point cloud data points fall within a given pixel, the pixel value of that point is selected based on the minimum distance. The specific steps include:

[0055] 1) The lidar acquires a frame of raw point cloud data Z at time t. t It records the position of each point cloud in the Cartesian coordinate system, Z. t It can be represented as:

[0056]

[0057] Where i is the index value of the point cloud, and N represents the number of points in a frame of the point cloud, (x i y i , z i This represents the coordinates of the point cloud at index i in the Cartesian coordinate system. First, the point cloud needs to be projected onto the spherical coordinate system using the following formula:

[0058]

[0059]

[0060]

[0061] Where d i θ represents the measured distance from the point cloud at index i to the lidar. i This represents the horizontal rotation angle of the point cloud at index i relative to the origin. This represents the vertical rotation angle of the point cloud at index i relative to the origin. The projected point cloud can be represented by a set. express:

[0062]

[0063] 3) Traversing the point cloud If a point θ is a point in the middle, then... i , Exceeding the preset point cloud range θ and Then remove it from the set.

[0064] 4) Based on the point cloud data in the spherical coordinate system and the pre-set front view angular resolution, traverse each point in the point cloud and calculate the front view coordinate index corresponding to each point:

[0065] P i_x =floor(θ) i ÷Δθ)

[0066]

[0067] Where P i_x P i_yThese represent the x-coordinate and y-coordinate indices of the point cloud corresponding to index i in the front view, respectively, with floor indicating rounding down. After determining the front view pixels corresponding to each point, the distance value d for each point... i_value Represented by the minimum point cloud distance falling within that pixel, the projection result is as follows: Figure 4 As shown:

[0068]

[0069] Step 4) is implemented using a vector, and the specific process is as follows:

[0070] ① Determine the width w and height h of the front view based on the predetermined point cloud range and resolution, and at the same time create a one-dimensional vector of size w×h and set all its values ​​to 0;

[0071] ② Traverse the points to obtain the index P of each point. i_x and P i_y According to the formula:

[0072] index = P i_y ×w+P i_x

[0073] Calculate the index of the point in the one-dimensional vector. If the value of the vector corresponding to that index is 0, directly set the distance d between the points. i Store the data in a vector. If the value is not zero, then combine the values ​​in the vector with the values ​​in the vector. i Smaller values ​​are stored in a vector.

[0074] 5) Traverse the vector to obtain the maximum distance d in the point cloud at time t. max and minimum value d min The distance from [d] is calculated using the following formula. min d max Normalize to [0, 255] to obtain the pixel value for each pixel:

[0075]

[0076] Where P i_value Represents the pixel value of a pixel.

[0077] Step 3: Calculate the fractional derivative for each pixel in the front view. This process is achieved by solving a one-dimensional convolution in the y-direction for each pixel. Specifically, it includes the following steps:

[0078] 1) The fractional derivative of the image in the y-direction can be expressed by the following formula:

[0079]

[0080] Where x and y are the indices of the image's horizontal and vertical coordinates, respectively; f(x, y) is the pixel value of the pixel indexed by x and y; and v represents the order of the fractional derivative, ranging from 0 to 1. Based on the above formula, solving for the fractional derivative for each pixel can be transformed into solving a one-dimensional convolution for each pixel and its neighboring pixels. The convolution kernel can be represented by the following one-dimensional matrix:

[0081]

[0082] 2) The formula for calculating fractional differentials can be expressed through the following matrix operations:

[0083]

[0084] Step 4: Determine the fractional derivative value of each pixel. If it is greater than a threshold, then all point clouds corresponding to that pixel can be identified as obstacle point clouds. This includes the following steps:

[0085] 1) According to Figure 2 and Figure 3 The result of the fractional derivative can be approximated as the difference between the normalized distance of the current scan line and the normalized distances of the two adjacent scan lines, based on the vertical resolution. And f(x, y) can be used to calculate the length of the arc l, where 1 indicates that the arc length is... Let f(x, y) be the arc length of the sector with the included angle and the radius f(x, y).

[0086] 2) The angle σ between the arc length and the plane can be obtained from the fractional derivative value and the arc length l. The calculation formula is as follows:

[0087]

[0088] according to Figure 2 Figure 3 It can be seen that if the ground is horizontal, the included angle σ is negative and close to -90°; if the obstacle is vertical, the included angle σ is positive. A threshold τ is set based on the robot's obstacle-crossing ability. When the included angle σ is less than τ, it is determined to be a non-obstacle; when the included angle σ is greater than τ, it is determined to be an obstacle. Figure 5 The results of obstacle detection in point clouds. Figure 6 This indicates the location of the obstacle in the camera.

[0089] Example

[0090] This embodiment is based on the following hardware platform: ① Data acquisition platform: CPU configuration is CoreTMi5-8259U; memory is DDR4-2400 1.2V SO-DIMM 16GB; storage is PCIe 3.0 Kioxia EXCERIA NVMe 500GB; camera is Hikvision USB camera DS-2CS54U0B-S; lidar is Livox HAP prism rotating lidar; the camera and lidar are connected to the NUC, and the NUC is connected to the mobile robot control chassis via USB. The power required by the device is provided by the lithium battery carried by the mobile robot itself.

[0091] ② Experimental platform: CPU configuration is Intel Core i5-11600K; memory is DDR4 16GB; storage is PCIe4.0 Western Digital SN770 1T.

[0092] This embodiment is based on the following software environment: computer operating system: Ubuntu 20.04, paired with ROS Noetic robot operating system.

[0093] Combination Figure 6 This invention relates to a lidar obstacle detection method based on fractional derivatives, and the specific implementation steps are as follows:

[0094] Step 1: Set the radar's horizontal and vertical angular resolution to 0.3°, and set the lidar's horizontal filtering range to [-45°, 45°] and vertical filtering range to [-12.3°, -0.3°]. Load the raw point cloud data according to the timestamp. The front view projection size is 300×40. Figure 1 The raw point cloud data loaded at a certain moment.

[0095] Step 2: Calculate the coordinate values ​​of the point cloud data projected from the Cartesian coordinate system onto the spherical coordinate system, and filter out the point clouds that are not in the range based on the projected coordinate values ​​and the preset horizontal and vertical filtering ranges in Step 1.

[0096] Step 3: Project the point cloud in the spherical coordinate system onto the front view according to the preset horizontal and vertical angular resolutions in Step 1, and use the normalized result of the minimum point cloud distance in each pixel as the pixel value of that point. Figure 4 This is the result of the front view projection.

[0097] Step 4: Set the order of the fractional derivative to 0.2, i.e., v = 0.2. Unfold the front view element by element into a matrix of size [3, 12000]. Set the convolution kernel to [1, -0.2, -0.08] based on the order of the fractional derivative. Perform matrix multiplication between the convolution kernel and the unfolded matrix to obtain a matrix of size [1, 12000]. Its value represents the fractional derivative value of 12000 pixels.

[0098] Step 5: Based on the fractional derivative value stored for each pixel and the distance value of the point cloud stored in the front view, calculate the angle between the arc length l and the adjacent plane of the point cloud, where l represents the vertical resolution with the point cloud distance as the radius. Let be the arc length of the sector formed by the included angle. The size of the included angle is used to determine whether adjacent planes constitute obstacles. Figure 5 The results of obstacle detection in point clouds. Figure 6 This represents the corresponding position of the obstacle in the camera.

Claims

1. A method for obstacle detection based on fractional derivatives in a three-dimensional point cloud forward projection map, characterized in that, Includes the following steps: Step 1: Based on the acquisition resolution of the LiDAR, set the horizontal and vertical angular resolution represented by each pixel in the front view to determine the horizontal and vertical range of the selected point cloud; Step 2: Filter the point cloud within the selected area, project the filtered point cloud onto the corresponding pixel in the front view according to its horizontal and vertical offsets, and use the normalized result of the distance from the point cloud to the LiDAR as the pixel value of that point. If multiple point cloud data fall into a single pixel in the front view, the pixel value of that point is selected as the value with the smallest distance. Step 3: Calculate the fractional derivative for each pixel in the front view. This process is achieved by solving the one-dimensional convolution in the y-direction for each pixel. Step 4: Determine the fractional derivative value of each pixel. If it is greater than a threshold, then all point clouds corresponding to that pixel are determined to be obstacle point clouds. The method for calculating the fractional derivative of each pixel in the front view using one-dimensional convolution in the third step is as follows: 1) Construct a two-dimensional matrix of type Eigen with a size of 3 rows and w×h columns. Use cv::Mat to read and save a front view. Iterate through each point in the front view and save the pixel value of each point and the pixel values ​​of the points in the same position in the first two rows, a total of 3 values, into the two-dimensional matrix of type Eigen. If the first two rows are empty, fill them with 0. 2) The fractional derivative of the image in the y-direction is expressed by the following formula: Where x and y are the index values ​​of the horizontal and vertical coordinates of the image, respectively, f(x, y) is the pixel value of the pixel with index x and y, and v represents the order of the fractional derivative, ranging from 0 to 1; construct a one-dimensional matrix of type Eigen, with a size of 1 row and 3 columns, and the matrix is ​​represented by the following formula: 3) Multiply the 3-row, w×h-column matrix obtained in 1) and the 1-row, 3-column matrix obtained in 2), using the following formula: Finally, a matrix with 1 row and (w×h) columns is obtained, which is saved in an image of size w×h, and is the fractional derivative value of each pixel; The specific method for determining obstacles in the fourth step, based on the fractional derivative, is as follows: 1) Solve for the angle σ of the right triangle with the difference between the opposite sides and l as the adjacent side, where l represents the angle with vertical resolution. Let be the included angle, and the arc length of the sector with the distance within the point cloud of the currently calculated pixel as its radius. The arc length can be approximated as a straight line. The specific formula for solving this problem is as follows: 2) When the value of the angle σ is negative and close to -90°, it means that the line connecting the point cloud in this pixel and the point cloud in the neighboring pixel is parallel to the horizontal road surface; when the angle σ is positive, it means that the line connecting the point cloud in this pixel and the point cloud in the neighboring pixel is parallel to a vertical plane; a threshold τ is set according to the robot's obstacle crossing ability. When the angle σ is less than τ, it is determined to be a non-obstacle, and when the angle σ is greater than τ, it is determined to be an obstacle.

2. The obstacle detection method for three-dimensional point cloud forward projection map based on fractional derivatives according to claim 1, characterized in that, The specific method for projecting the filtered point cloud data onto the front view in the second step is as follows: 1) Set the horizontal resolution Δθ and vertical resolution of the front view according to the acquisition resolution of the lidar. The angular resolution of the front view is set to be greater than the acquisition resolution of the lidar; 2) The lidar acquires a frame of raw point cloud data at time t, which is represented by a set Z. t express Where i is the index value of the point cloud, and N represents the number of points in a frame of the point cloud, (x i ,y i ,z i () represents the coordinates of the point cloud at index i in the Cartesian coordinate system; the coordinates of the point cloud in the Cartesian coordinate system are projected to the spherical coordinate system according to the following formula: Where, d i θ represents the measured distance from the point cloud at index i to the lidar. i This represents the horizontal rotation angle of the point cloud at index i relative to the origin. This represents the vertical rotation angle of the point cloud at index i relative to the origin; the projected point cloud is represented by a set. express: 3) Filter out point clouds that do not belong to the preset range, and calculate the front view coordinate index corresponding to each point based on the filtered point cloud data and the preset front view angular resolution: P i_x =floor(θ i ÷Δθ) Among them, P i_x P i_y Let represent the x-coordinate index and y-coordinate index of the point cloud corresponding to index i in the front view, respectively, with floor indicating rounding down; after determining the front view pixels corresponding to each point, the distance value d of each point is... i_value Represented by the minimum point cloud distance falling within that pixel:

3. The obstacle detection method for three-dimensional point cloud forward projection map based on fractional derivatives according to claim 1, characterized in that, The front view properties in step two are shown in the table below: The horizontal rotation angle range is used to filter point clouds located within the horizontal rotation angle range, with values ​​of [-45°, 45°]. The vertical rotation angle range is used to filter point clouds located within the vertical rotation angle range, with values ​​of [-12.3°, -0.3°]. The horizontal angular resolution is used to filter point clouds within the horizontal rotation angle range, and its value is 0.3°. Vertical angular resolution, which is the range of vertical angles represented by each pixel, with a value of 0.3°; Image size, with a value of 300×40.

Images