Normal distributions transform (NDT) method for LiDAR point cloud localization in unmanned driving

Abstract

A normal distributions transform (NDT) method for LiDAR point cloud localization in unmanned driving is provided. The method proposes a non-recursive, memory-efficient data structure occupation-aware-voxel-structure (OAVS), which speeds up each search operation. Compared with a tree-based structure, the proposed data structure OAVS is easy to parallelize and consumes only about 1/10 of memory. Based on the data structure OAVS, the method proposes a semantic-assisted OAVS-based (SEO)-NDT algorithm, which significantly reduces the number of search operations, redefines a parameter affecting the number of search operations, and removes a redundant search operation. In addition, the method proposes a streaming field-programmable gate array (FPGA) accelerator architecture, which further improves the real-time and energy-saving performance of the SEO-NDT algorithm. The method meets the real-time and high-precision requirements of smart vehicles for three-dimensional (3D) lidar localization.

Description

CROSS REFERENCE TO THE RELATED APPLICATIONS

This application is the national phase entry of International Application No. PCT/CN2021/119506, filed on Sep. 22, 2021, which is based upon and claims priority to Chinese Patent Application No. 202110718013.2, filed on Jun. 28, 2021, the entire contents of which are incorporated herein by reference.

TECHNICAL FIELD

The present disclosure relates to a three-dimensional (3D) point cloud localization technology, and in particular to a normal distributions transform (NDT) method for LiDAR point cloud localization in unmanned driving.

BACKGROUND

The normal distributions transform (NDT) algorithm is widely used to register two-dimensional (2D) or three-dimensional (3D) point clouds in smart vehicles, 3D reconstruction, motion estimation, object detection and pose estimation, simultaneous localization and mapping (SLAM), etc. In the field of smart vehicles, localization is the most basic and important task, and the NDT-based localization system is widely used in autonomous driving systems using a lidar as the main sensor.

The NDT algorithm segments a target point cloud into a series of grids and uses a normal distribution to represent the distribution of points in each point cloud. It converts point cloud to point cloud registration into point cloud to normal distribution registration, thereby improving the speed and robustness of the algorithm.

The current autonomous driving system usually uses a lidar of 64, 128 or more channels as the main sensor of the localization algorithm. The sampling frequency of the lidar is gradually increased, and will reach 30 Hz in the future. In other words, the number of points input by the lidar per second will be close to one million, which poses a great challenge to the real-time performance of the localization system. A test shows that the traditional NDT algorithm can only reach the input frequency of 2 Hz on an embedded advanced reduced instruction-set computer (RISC) machine (ARM) platform, which is far from meeting the real-time requirement.

To this end, researchers are pursuing two primary goals. The first goal is to reduce the number of search iterations. Reference [1] proposed a multi-layered NDT algorithm to represent point clouds in order to reduce the number of iterations and measure longer distances. However, updating the NDT of all layers requires too much memory and unacceptable time. Reference [2] made improvements on Reference [1] and proposed a key-layered NDT algorithm, which only needs to search the key layer to satisfy the termination conditions of higher layers. Unfortunately, this method cannot meet the real-time requirement. The second goal is to reduce the running time per iteration. References [3] and [4] extended the point-to-distribution (P2D)-NDT to distribution-to-distribution (D2D)-NDT, thereby transforming a set of points into a distribution to reduce the running time per iteration. However, the D2D-NDT method suffers from poor accuracy and slow speed when dealing with massive and non-uniform lidar point clouds in smart vehicles. Reference [5] proposed semantic-assisted (SE)-NDT to classify point clouds and remove dynamic objects by using segmentation methods [6], [7] and [8]. However, the method proposed by Reference [5] can only remove a limited number of points and requires additional time for point cloud segmentation. Overall, these methods lack real-time performance when dealing with large point clouds.

With the increase in the number of point clouds generated by the new generation of lidar and the increase in the running speed of autonomous vehicles, it is increasingly difficult for the NDT algorithm running on the vehicle-mounted industrial computer to meet the real-time requirement. In addition, the strict constraint on the use of batteries by smart vehicles also puts forward an energy consumption requirement for the NDT algorithm. In this context, it is necessary to design an improved NDT algorithm, based on the distribution of laser point clouds in an autonomous driving scenario, to accelerate the localization process through an energy-efficient heterogeneous computing device, such as a field-programmable gate array (FPGA) or a graphics processing unit (GPU).

• [1] C. Ulas and H. Temeltas, “A fast and robust feature-based scan-matching method in 3d slam and the effect of sampling strategies,” 2013.
• [2] H. Hong and B. H. Lee, “Key-layered normal distributions transform for point cloud registration,” Electronics Letters, vol. 51, no. 24, p. 1986-1988, 2015.
• [3] T. Stoyanov, M. Magnusson, H. Andreasson, and A. J. Lilienthal, “Fast and accurate scan registration through minimization of the distance between compact 3d ndt representations,” The International Journal of Robotics Research, vol. 31, no. 12, p. 1377-1393, October 2012.
• [4] S. Lam, G. Jiang, M. Wu, and B. Cao, “Area-time efficient streaming architecture for fast and brief detector,” IEEE Transactions on Circuits and Systems II: Express Briefs, vol. 66, no. 2, pp. 282-286, February 2019.
• [5] A. Zaganidis, A. Zerntev, T. Duckett, and G. Cielniak, “Semantically assisted loop closure in slam using ndt histograms,” 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2019.
• [6] L. Bai, Y Lyu, and X. Huang, “Roadnet-rt: High throughput cnn architecture and soc design for real-time road segmentation,” IEEE Transactions on Circuits and Systems I: Regular Papers, vol. 68, no. 2, p. 704-714, February 2021.
• [7] X. Chang, H. Pan, W. Lin, and H. Gao, “A mixed-pruning based framework for embedded convolutional neural network acceleration,” IEEE Transactions on Circuits and Systems I: Regular Papers, p. 1-10, 2021.
• [8] H. Zhang, Y Shu, W. Jiang, Z. Yin, W. Zhao, and Y Ha, “A 55 nm, 0.4v 5526-tops/w compute-in-memory binarized cnn accelerator for aiot applications,” IEEE Transactions on Circuits and Systems II: Express Briefs, pp. 1-1, 2021.

SUMMARY

In order to meet the requirements of unmanned driving for real-time and efficient localization data, the present disclosure proposes a normal distributions transform (NDT) method for LiDAR point cloud localization in unmanned driving. The present disclosure establishes a non-recursive, memory-efficient data structure occupation-aware-voxel-structure (OAVS), which meets the real-time and high-precision requirements of smart vehicles for three-dimensional (3D) lidar localization.

The present disclosure adopts the following technical solution: an NDT method for LiDAR point cloud localization in unmanned driving. The method specifically includes the following steps:

• • 1) establishing a data structure OAVS:
  • 1.1) defining a point cloud input by a lidar in a previous scan as a fixed point cloud P_F, and calculating, according to the input fixed point cloud P_F, an axially aligned bounding box of the input fixed point cloud P_F;
  • 1.2) segmenting, according to a preset scale parameter R₁, the axially aligned bounding box calculated in step 1.1) into a series of 3D spatial volumes with a side length of R₁, and defining each of the 3D spatial volumes with a side length of R₁ as a voxel; and
  • 1.3) calculating a count of points in each voxel; segmenting a voxel with a count of points exceeding a threshold Th_p into a series of 3D spatial volumes with a side length of R₂, R₂<R₁; and defining each of the 3D spatial volumes with a side length of R₂ as a sub-voxel, which, together with the voxel, forms the data structure OAVS; and
  • 2) acquiring NDT information of the fixed point cloud based on the data structure OAVS, and optimizing a search operation to improve a real-time performance of an NDT algorithm:
  • 2.1) segmenting the fixed point cloud P_F, and extracting, by an open-source neural network Cylinder3D for point cloud segmentation, different classes of objects identified in the fixed point cloud;
  • 2.2) acquiring the NDT information of the fixed point cloud P_F, and calculating the NDT information of each class of point set in each sub-voxel in the data structure OAVS corresponding to the fixed point cloud P_F, namely a mean and a variance for each class in each sub-voxel;
  • 2.3) registering a moving point cloud P_M with the fixed point cloud P_F having the NDT information acquired:
  • performing the following operations for each point p_Mⁱ in the moving point cloud:
  • 2.3.1) transforming, according to a pose transformation matrix T, the point p_Mⁱ into a coordinate system of the fixed point cloud to acquire a transformed point {circumflex over (p)}_Mⁱ=T×p_Mⁱ, thereby acquiring a registered moving point cloud {circumflex over (p)}_Mⁱ in the coordinate system of the fixed point cloud;
  • 2.3.2) searching, in the sub-voxel of the fixed point cloud, a sub-voxel o where {circumflex over (p)}_Mⁱ is located, and calculating, based on a class j to which {circumflex over (p)}_Mⁱ belongs, a probability that {circumflex over (p)}_Mⁱ matches a normal distribution of the sub-voxel:

$\rho \left({\hat{p}}_M^i\right)=\frac{1}{\sqrt{{\left(2\pi \right)}^3{\left(\sum \right)}_{\mathrm{oj}}}}\exp \left(-\frac{{\left({\hat{p}}_M^i-{\mu }_{\mathrm{oj}}\right)}^T{\left(\sum \right)}_{\mathrm{oj}}^{-1}\left({\hat{p}}_M^i-{\mu }_{\mathrm{oj}}\right)}{2}\right),$ where, ({circumflex over (p)}_Mⁱ−μ_oj)^TΣ_o⁻¹({circumflex over (p)}_Mⁱ−μ_oj) represents a transpose matrix of ({circumflex over (p)}_Mⁱ−μ_oj) multiplied by an inverse matrix of Σ_oj and multiplied by ({circumflex over (p)}_Mⁱ−μ_oj); μ_ij represents the mean of the class j in the sub-voxel o; and Σ_oj represents the variance of the class j in the sub-voxel o;

• • 2.3.3) acquiring a total matching score by

$\mathrm{score}=\sum _{p_M^i\in P_M}\rho \left(T\times p_M^i\right)$ according to the probability calculated in step 2.3.2), where, the pose transformation matrix T is solved by a Newton-Gauss iteration method in order for an optimal pose transformation matrix T and an optimal matching score;

• • 2.3.4) denoting a first derivative gradient matrix of the score function as {right arrow over (g)} and a second derivative Hessian matrix thereof as H;
  • 2.3.5) solving HΔT=−{right arrow over (g)} to obtain an increment ΔT of a pose rotation matrix T;
  • 2.3.6) updating the pose rotation matrix T by T←T+ΔT;
  • 2.3.7) returning to step 2.3.1) if the score function does not converge below a desired threshold, that is, a matching degree between the moving point cloud P_M and the fixed point cloud P_F is not as desired, redefining the pose transformation matrix T by the pose rotation matrix T updated in step 2.3.6), and repeating steps 2.3.1) to 2.3.6) until the score function converges below the desired threshold; and
  • 2.3.8) taking a final pose transformation matrix T obtained in step 2.3.7) as the pose transformation matrix of the moving point cloud relative to the fixed point cloud.

Further, in step 2.2), during acquiring the NDT information, the segmented fixed point cloud may be streamed into an FPGA accelerator to establish the data structure OAVS, that is, to perform step 1), and to calculate the NDT information of each sub-voxel.

Further, in step 2.3), in a registration stage, the segmented moving point cloud may be streamed into the FPGA accelerator to be efficiently processed in the form of data stream and pipeline, so as to obtain the gradient matrix and the Hessian matrix.

The present disclosure has the following beneficial effects. The present disclosure proposes a non-recursive, memory-efficient data structure OAVS that speeds up each search operation. Compared with a tree-based structure, the proposed data structure OAVS is easy to parallelize and consumes only about 1/10 of memory. Based on the data structure OAVS, the present disclosure proposes a semantic-assisted OAVS-based (SEO)-NDT algorithm, which significantly reduces the number of search operations, redefines a parameter affecting the number of search operations, and removes a redundant search operation. In addition, the present disclosure proposes a streaming FPGA accelerator architecture, which further improves the real-time and energy-saving performance of the SEO-NDT algorithm.

BRIEF DESCRIPTION OF THE DRAWINGS

The FIGURE is a diagram of a collaboration framework of a normal distributions transform (NDT) method for LiDAR point cloud localization in unmanned driving according to the present disclosure.

DETAILED DESCRIPTION OF THE EMBODIMENTS

The present disclosure will be described in detail below with reference to the drawings and specific embodiments. The embodiments are implemented on the premise of the technical solutions of the present disclosure. The following presents detailed implementations and specific operation processes. The protection scope of the present disclosure, however, is not limited to the following embodiments.

The present disclosure provides a normal distributions transform (NDT) method for LiDAR point cloud localization in unmanned driving, which is implemented on a field-programmable gate array (FPGA) through a high-level synthesis tool. The present disclosure establishes a non-recursive, memory-efficient data structure occupation-aware-voxel-structure (OAVS), a real-time semantic-assisted OAVS-based (SEO)-NDT algorithm based on OAVS, and a streaming FPGA accelerator architecture. The method specifically includes the following steps:

• • 1) Establish a data structure OAVS:
  • 1.1. Calculate, according to the input fixed point cloud P_F, an axially aligned bounding box (AABB) of an input fixed point cloud P_F.
  • 1.2) Segment, according to a preset scale parameter R₁, the axially aligned bounding box calculated in the previous step into a series of 3D spatial volumes with a side length of R₁, and define each of the 3D spatial volumes with a side length of R₁ as a voxel.
  • 1.3. Calculate a count of points in each voxel; segment a voxel with a count of points exceeding a threshold Th_p into a series of 3D spatial volumes with a side length of R₂, R₂<R1; and define each of the 3D spatial volumes with a side length of R₂ as a sub-voxel, which, together with the voxel, forms the data structure OAVS.
  • 2. Establish an SEO-NDT algorithm based on the data structure OAVS:
  • 2.1 Define a point cloud input by a lidar in a previous scan as a fixed point cloud P_F: segment the fixed point cloud P_F, and extract, by an open-source neural network Cylinder3D for point cloud segmentation, different classes of objects identified in the fixed point cloud. Here, the point cloud is segmented into 24 classes, namely 1) vehicles, 2) bicycles, 3) motorcycles, 4) trucks, 5) other vehicles, 6) people, 7) cyclists, 8) motorcyclists, 9) road surfaces, 10) stop lines, 11) zebra crossings, 12) other ground surfaces, 13) buildings, 14) fences, 15) green vegetation, 16) tree trunks, 17) ramps, 18) road signs, 19) traffic signs, 20) moving vehicles, 21) moving people, 22) moving bicycles, 23) moving trucks, and 24) other moving vehicles.
  • 2.2 Acquire NDT information of the fixed point cloud P_F: calculate the NDT information of each class of point set in each sub-voxel in the data structure OAVS corresponding to the fixed point cloud P_F, namely a mean and a variance for each class in each sub-voxel, where the mean is

${\mu }_{\mathrm{oj}}=\frac{1}{m}\sum _{k=1}^m{p}_{\mathrm{koj}},$ and the variance is

${\left(\sum \right)}_{\mathrm{oj}}=\frac{1}{m-1}\sum _{k=1}^m\left(p_{\mathrm{koj}}-{\mu }_{\mathrm{oj}}\right){\left(p_{\mathrm{koj}}-{\mu }_{\mathrm{oj}}\right)}^T;$ the subscript o represents an o-th sub-voxel; the subscript j represents a j-th class; the subscript k represents a k-th point in the sub-voxel; p_koi represents coordinates (x,y,z) of the k-th point in a point set of the j-th class in the o-th sub-voxel; m represents a count of points in the point set of the j-th class in the o-th sub-voxel; and (p_koi−μ_oi)^T represents a transpose matrix of (p_koi−μ_oi). If there are multiple classes of point clouds in a certain sub-voxel, the NDT information of all classes of point sets in the sub-voxel is calculated. That is, the counts of means and variances obtained are corresponding to the count of classes of the point clouds in the sub-voxel.

• • 2.3 Register a moving point cloud P_M with the fixed point cloud P_F having the NDT information acquired: perform the following operations for each point p_Mⁱ in the moving point cloud:
  • 2.3.1 Transform, according to a pose transformation matrix T, the point p_Mⁱ into a coordinate system of the fixed point cloud to acquire a transformed point {circumflex over (p)}_Mⁱ=T×p_Mⁱ, thereby acquiring a registered moving point cloud {circumflex over (p)}_Mⁱ in the coordinate system of the fixed point cloud.
  • 2.3.2 Search, in the sub-voxel of the fixed point cloud, a sub-voxel o where {circumflex over (p)}_Mⁱ is located, and calculating, based on a class j to which {circumflex over (p)}_Mⁱ belongs, a probability that {circumflex over (p)}_Mⁱ matches a normal distribution of the sub-voxel:

$\rho \left({\hat{p}}_M^i\right)=\frac{2}{\sqrt{{\left(2\pi \right)}^3{\left(\sum \right)}_{\mathrm{oj}}}}\exp \left(-\frac{{\left({\hat{p}}_M^i-{\mu }_{\mathrm{oj}}\right)}^T{\left(\sum \right)}_{\mathrm{oj}}^{-1}\left({\hat{p}}_M^i-{\mu }_{\mathrm{oj}}\right)}{2}\right),$ where, ({circumflex over (p)}_Mⁱ−μ_oj)^TΣ_o⁻¹({circumflex over (p)}_Mⁱ−μ_oj) represents a transpose matrix of ({circumflex over (p)}_Mⁱ−μ_oj) multiplied by an inverse matrix of Σ_oj and multiplied by ({circumflex over (p)}_Mⁱ−μ_oj).

• • 2.3.3 Acquire a total matching score by

$\mathrm{score}=\sum _{p_M^i\in P_M}\rho \left(T\times p_M^i\right)$ according to the probability, where, the pose transformation matrix T is solved by a Newton-Gauss iteration method in order for an optimal pose transformation matrix T and an optimal matching score.

• • 2.3.4 Denote a first derivative gradient matrix of the score function as {right arrow over (g)} and a second derivative Hessian matrix thereof as H.
  • 2.3.5 Solve HΔT=−{right arrow over (g)} to obtain an increment ΔT of a pose rotation matrix T.
  • 2.3.6 Update the pose rotation matrix T by T←T+ΔT.
  • 2.3.7 Return to step 2.3.1 if the score function does not converge below a desired threshold, that is, a matching degree between the moving point cloud P_M and the fixed point cloud P_F is not as desired, redefine the pose transformation matrix T by the pose rotation matrix T updated in step 2.3.6, and repeat steps 2.3.1 to 2.3.6 until the score function converges below the desired threshold.
  • 2.3.8 Take a final pose transformation matrix T obtained in step 2.3.7 as the pose transformation matrix of the moving point cloud relative to the fixed point cloud.

The pseudocode of the algorithm is as follows:

Algorithm 1: SEO-NDT algorithm
for point cloud registration
1.  Input: fixed point cloud PF, moving point
    cloud PM, initial transformation matrix Tinit,
    point cloud segmenter D
2.  Output: transformation matrix T, T×(PM)→PF
3.  {segment:};
4.  Mark PF and PM by D, and obtain point
    clouds LF and LM marked by D;
5.  {initialize:};
6.  Establish OAVS O for PF;
7.  forall marked point pFi ∈ PF, do;
8.  | Search sub-voxel oi ∈ O including pFi;
9.  | Update μo and Σo of oi;
10. end
11. {register:};
12. Initialize the transformation matrix: T←Tinit;
13. while the function does not converge, do
14. | score←0, g→0, H←0;
15. | forall, mark class d ∈ D, do
16. | | forall, class-d point pMi ∈ PM, do
17. | | | Transform pMi to the coordinate
    | | | system the fixed point cloud
    | | | by the transformation
    | | | matrix T, {circumflex over (p)}Mi = T × pMi;
18. | | | Search sub-voxel oi where {circumflex over (p)}Mi is located;
19. | | | Calculate probability p({circumflex over (p)}Mi);
20. | | | Update g→ and H;
21. | | end
22. | end
23. | Solve HΔT = −g→; T ← T + ΔT ;
24. end

The streaming FPGA accelerator architecture is shown in the FIGURE. During acquiring the NDT information, the segmented fixed point cloud is streamed into an FPGA accelerator to establish the data structure OAVS and calculate the NDT information of each sub-voxel. In a registration stage, the segmented moving point cloud is streamed into the FPGA accelerator to be efficiently processed in the form of data stream and pipeline, so as to obtain the gradient matrix and the Hessian matrix.

The above described are merely several embodiments of the present invention. Although these embodiments are described specifically and in detail, they should not be construed as a limitation to the patent scope of the present disclosure. It should be noted that those of ordinary skill in the art may further make several variations and improvements without departing from the idea of the present disclosure, but such variations and improvements should all fall within the protection scope of the present disclosure. Therefore, the protection scope of the present disclosure should be subject to the appended claims.

Claims

1. A normal distributions transform (NDT) method for LiDAR point cloud localization in unmanned driving, wherein the NDT method is performed by field-programmable gate array (FPGA), comprising the following steps: 1) Establishing, by the FPGA, a data structure occupation-aware-voxel-structure (OAVS):1.1) defining a point cloud input by a lidar in a previous scan as a fixed point cloud pF nd calculating, according to the input fixed point cloud pF, an axially aligned bounding box of the input fixed point cloud pF;1.2) segmenting, according to a preset scale parameter R1, the axially aligned bounding box calculated in step 1.1) into a series of three-dimensional (3D) spatial volumes with a side length of R1, and defining each of the 3D spatial volumes with the side length of R1 as a voxel; and1.3) calculating a count of points in each voxel; segmenting a voxel with a count of points exceeding a threshold Thp into a series of 3D spatial volumes with a side length of R2, wherein R2<R1; and defining each of the 3D spatial volumes with the side length of R2 as a sub-voxel, which, together with the voxel, forms the data structure OAVS; and2) Acquiring, by the FPGA, NDT information of the fixed point cloud based on the data structure OAVS, and optimizing a search operation to improve a real-time performance of an NDT algorithm:2.1) segmenting the fixed point cloud pF, and extracting, by an open-source neural network Cylinder3D for point cloud segmentation, different classes of objects identified in the fixed point cloud;2.2) acquiring the NDT information of the fixed point cloud pF, and calculating NDT information of each class of a point set in each sub-voxel in the data structure OAVS corresponding to the fixed point cloud pF, namely a mean and a variance for each class in each sub-voxel;2.3) registering a moving point cloud pM with the fixed point cloud pF having the NDT information acquired:performing the following operations for each point piM in the moving point cloud:2.3.1) transforming, according to a pose transformation matrix T, the point piM into a coordinate system of the fixed point cloud to acquire a transformed point {circumflex over (p)}iM=T×piM, thereby acquiring a registered moving point cloud {circumflex over (p)}iM in the coordinate system of the fixed point cloud;2.3.2) searching, in the sub-voxels of the fixed point cloud, a sub-voxel o where {circumflex over (p)}iM is located, and calculating, based on a class j to which {circumflex over (p)}iM belongs, a probability that {circumflex over (p)}iM matches a normal distribution of the sub-voxel:

2. The NDT method for LiDAR point cloud localization in unmanned driving according to claim 1, wherein in step 2.2), during acquiring the NDT information, a segmented fixed point cloud is streamed into a field-programmable gate array (FPGA) accelerator to establish the data structure OAVS, that is, to perform step 1), and to calculate the NDT information of each sub-voxel.

3. The NDT method for LiDAR point cloud localization in unmanned driving according to claim 2, wherein in step 2.3), in a registration stage, a segmented moving point cloud is streamed into the FPGA accelerator to be efficiently processed in a form of a data stream and a pipeline to obtain the gradient matrix and the Hessian matrix.