Real-time laser radar point cloud 3d mapping method implemented on FPGA

Abstract

The invention relates to a real-time laser radar point cloud 3d mapping method implemented on an FPGA (Field Programmable Gate Array), which is compatible with accuracy, efficiency and power consumption, and constructs a laser radar point cloud coordinate conversion algorithm based on the parallel capability of the FPGA to realize the acceleration processing of laser radar data coordinate conversion by the FPGA; violent nearest neighbor search with a nearest neighbor threshold value introduced is used as a nearest neighbor calculation method; the FPGA configures a processing element array for parallel calculation of the distance between point pairs according to a violent nearest neighbor search optimization data cache architecture with a nearest neighbor threshold introduced, and the violent nearest neighbor search is accelerated by hardware; the FPGA executes a P2P-ICP algorithm, point cloud alignment is carried out through violent nearest neighbor search, attitude parameters are generated, then the generated attitude parameters are used as attitude priori estimation, and Gaussian-Newton algorithm iteration is carried out through an improved ICP stage to obtain a final attitude; and the FPGA adopts a laser radar data processing mode of fixed-point number operation.

Description

Technical Field

[0001] The present invention relates to the technical field of lidar point cloud mapping, and particularly to a real-time lidar point cloud 3D mapping method implemented on an FPGA. Background Art

[0002] Three-dimensional lidar sensors are widely used in the positioning, perception, and mapping functions of autonomous vehicles. However, the high computational processing requirements of three-dimensional lidar sensors remain a significant drawback. Although high-end graphics processing units (GPUs) and central processing units (CPUs) can solve this problem, cost and energy consumption hinder the practical application of three-dimensional lidar sensors in commercial vehicles. When mobile platforms (such as robots, autonomous vehicles, etc.) operate in an initially unknown environment, a standard point cloud map is required. At the same time, the positioning algorithm constructs a spatial representation of the environment by estimating the positions of observed landmarks in the point cloud data and predicts the pose and position of the mobile platform. All spatial representations are represented in a pre-selected world reference system, usually originating from the initial coordinates of the mobile platform. A key aspect of constructing a mobile platform map is to quickly and accurately determine the pose and position of the mobile platform, especially in applications such as autonomous vehicles that require high-speed driving. The computational speed of the map construction algorithm is crucial because it involves a large amount of calculations to quickly search for point-to-point minima in two frames of point cloud data to determine the position of the mobile platform. The autonomy of the mobile platform is limited by the power consumption of the platform used. Therefore, the challenge lies in achieving a balance between computational resources and processing speed while ensuring accurate algorithm results in an energy-efficient manner, and this challenge currently constitutes a technical problem. Summary of the Invention

[0003] In order to solve the above technical problems or at least partially solve the above technical problems, the present invention provides a real-time lidar point cloud 3D mapping method implemented on an FPGA.

[0004] The present invention provides a real-time lidar point cloud 3D mapping method implemented on an FPGA, including: Construct a lidar point cloud coordinate conversion algorithm based on the parallel capabilities of the FPGA to achieve FPGA-accelerated lidar data coordinate conversion processing; Use brute force nearest neighbor search with a nearest neighbor threshold as the nearest neighbor calculation method; the FPGA optimizes the data cache architecture for brute force nearest neighbor search with a nearest neighbor threshold and configures an array of processing elements for parallel distance calculation between point pairs to hardware-accelerate brute force nearest neighbor search with a nearest neighbor threshold; The FPGA executes the P2P-ICP algorithm, which uses a brute-force nearest neighbor search with a nearest neighbor threshold to align point clouds and generate pose parameters. The generated pose parameters are then used as pose prior estimates, and the final pose is obtained by iterating the Gauss-Newton algorithm through the improved ICP stage. FPGA uses fixed-point arithmetic to process lidar data.

[0005] Furthermore, the efficient LiDAR point cloud coordinate conversion algorithm based on FPGA parallel capabilities includes: After obtaining the calibration parameters transmitted by the device information output protocol when the system starts, the FPGA calculates the calibration parameters and the sine and cosine coefficients related to the pitch angle and azimuth angle of the lidar, and stores the calibration parameters and sine and cosine coefficients in the BRAM; the calibration parameters include: the horizontal error angle of the starting point of the lidar channel beam , the horizontal installation angle of the laser radar channel , the installation position offset of the laser radar relative to the X axis , the installation position offset of the laser radar relative to the Z axis ; During the coordinate conversion process of the LiDAR point cloud data in parallel on the FPGA, the pre-stored parameters are read from the BRAM during system initialization: ; First, the FPGA uses the acquired parameters to calculate the following intermediate variables in parallel: ; , , , , , ; FPGA then calculates the intermediate variables based on the first step of parallel operation. Parallel calculation of the following intermediate variables : , ; Finally, the FPGA calculates the coordinates in parallel based on the intermediate variables of the first and second steps: , , .

[0006] Further, the brute-force nearest neighbor search introducing the nearest neighbor threshold includes: Calculating the position coordinates of the current point in the source point cloud and the Euclidean distance between the position coordinates of each point in the target point cloud; If and the Euclidean distance between is greater than the given nearest neighbor threshold, then the current point in the source point cloud does not match the point in the target point cloud; If and the Euclidean distance between is less than or equal to the given nearest neighbor threshold, then further check and the Euclidean distance between whether it is less than or equal to the Euclidean distance between the point in the target point cloud and the current best matching point If so, update the best matching point of the point in the target point cloud to , and update the Euclidean distance between it and the best matching point. If not, the best matching point of the point in the target point cloud remains unchanged; Among them, the matching completion condition is that when the output meets the preset nearest neighbor threshold, the matching is completed only after continuing to search for the subsequent "set number" of point cloud data; Execute the above process for all the point cloud points in the source point cloud to find all the best matching point pairs.

[0007] Further, during the process of writing point cloud data into the DDR, optimize the data cache architecture to control the data flow to perform data framing according to the requirements of the processing element array. The first frame is marked with a "key frame mark", and the first frame is given an initial rotation parameter; other cached data frames are marked as "data marks"; subsequently, new "key frame marks" are added according to the displacement amount △t of the "data mark" frame relative to the key frame; the FPGA directly completes low-latency parametric rotation through combinational logic when reading the point cloud coordinate data in the DDR cache.

[0008] Further, the processing element array of the PGA preliminarily preloads the map point cloud data and real-time point cloud data required for brute-force nearest neighbor search introducing the nearest neighbor threshold, and sequentially extracts the point cloud data for parallel calculation of Euclidean distance; after calculating the Euclidean distance, point pair matching is performed according to the brute-force nearest neighbor search introducing the nearest neighbor threshold; the calculated matching point pairs are provided to the registration module, and the registration module outputs the pose parameters according to the positions of the paired source point cloud and target point cloud, and the pose includes the rotation matrix R and the translation matrix t.

[0009] Further, when the displacement amount △t relative to the key frame exceeds the "set number", the current calculation data marker frame will be marked as a key frame marker; during the matching process, when the displacement amount △t relative to the key frame does not reach the set number, the system will clear the current data and buffer information, but will retain the current pose parameters as the iterative initial parameters for subsequent data frames; when the cumulative number of key frames exceeds the'maximum number of key frames', the earliest key frame will be transmitted to the host computer via Ethernet in chronological order, and at the same time, its pose parameters will be output for subsequent 3D mapping.

[0010] Further, the improved ICP stage of the P2P-ICP algorithm includes: S10, using the generated pose parameters as the pose prior estimate: ; S20, setting the pose parameters at any iteration step ; S30, at the pose parameters at any iteration step , using brute-force nearest neighbor search introducing the nearest neighbor threshold to determine the paired source point cloud points and target point cloud points: and ; S40, calculating the Hessian matrix H(x) and constructing the Gauss-Newton equation based on the Hessian matrix; S50, if the rank of the Hessian matrix H(x) is 6, then go to S60, otherwise the iteration ends; S60, decomposing the Hessian matrix H(x) and solving the Gauss-Newton equation; S70, if the solution of the Gauss-Newton equation then go to S80, otherwise go to S30, S80, according to update the pose parameters.

[0011] Further, the process of calculating the Hessian matrix H(x) and constructing the Gauss-Newton equation based on the Hessian matrix includes: S41, constructing an error function for point-to-point alignment: ; S42. Calculate the derivative of the error function with respect to the rotation matrix by right-multiplying the perturbation model: ; S43. Calculate the derivative of the error function with respect to : ; S44. Calculate the Hessian matrix H(x) and construct the Gauss-Newton equation: Construct the Jacobian matrix using the derivatives of the error function with respect to the rotation matrix and the translation matrix: ; Then the Hessian matrix is: ; The gradient vector of the error function is: ; The Gauss-Newton equation is: .

[0012] Furthermore, the decomposition of the Hessian matrix H(x) includes: The decomposition form of the Hessian matrix H(x) is: , where L is a unit lower triangular matrix and D is a diagonal matrix; The decomposition process is as follows: Initialization: , that is directly take the first diagonal element of the Hessian matrix H(x); , The first column of is the off-diagonal elements of the first column of the Hessian matrix H(x) divided by the first diagonal element; Recursively calculate and , w = 1, …, 5, i = w + 1, …, 5: [[ID=**69**]] , that is the current diagonal element minus the accumulation of all previous ; , that is the current off-diagonal element minus the accumulation of all previous and then divided by .

[0013] Furthermore, After decomposition, the Gauss-Newton equation becomes: , Let , , and solve it in the following three steps: Forward substitution to solve : Since is a unit lower triangular matrix, and its form is:

[0014] The solution of ; Diagonal scaling to solve : Since is a diagonal matrix, directly divide element by element: ; Backward substitution to solve : Since is a unit upper triangular matrix, and its form is: ; The equation The solution of is: , calculate in reverse order from the last row, and use the previously obtained Recursively .

[0015] The above technical solutions provided by the embodiments of the present invention have the following advantages compared with the prior art: This application designs an efficient lidar point cloud coordinate conversion algorithm based on the parallel capabilities of FPGA, constructs an energy-efficient data cache processing architecture based on FPGA, reduces the computational imbalance and latency caused by lidar data preprocessing, constructs a parallel operation method for the distance between point cloud points in the brute-force nearest neighbor search algorithm with a nearest neighbor threshold introduced based on an array of processing elements, and reduces the complexity of the brute-force nearest neighbor search algorithm with a nearest neighbor threshold introduced. Adding a nearest neighbor threshold improves the brute-force nearest neighbor search algorithm. When the matching is unsuccessful, it reduces the time doubling caused by multiple iterations. And to improve the accuracy of point cloud matching, a mechanism based on the nearest neighbor threshold is established to solve the minimum nearest neighbor value. In addition, to improve the computational efficiency, a fixed-point number operation method for lidar data processing is proposed. When lidar data is input, the original lidar data is quickly converted into coordinate data in the Cartesian coordinate system, and each frame of the point cloud is characterized by a quaternion (R, T). This method effectively solves the problem of wasting a large amount of storage resources and computational bandwidth by caching a large amount of iteratively converted data. Finally, the P2P-ICP algorithm is optimized to parallelize the algorithm process, improve the computational speed, and maintain high precision at the same time. The optimized brute-force nearest neighbor search algorithm of this application improves the point cloud registration speed by 900 times compared with the brute-force nearest neighbor search algorithm. Compared with the Kd tree, the optimized brute-force nearest neighbor search algorithm of this application has a faster speed within 5 iterations. It runs in real time on FPGA to obtain a higher energy efficiency ratio and lower cost. It can maintain high precision when processing point cloud data in real time. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] The accompanying drawings herein are incorporated into and constitute a part of this specification, showing embodiments consistent with the present invention, and are used together with the specification to explain the principles of the present invention.

[0017] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the drawings required for use in the description of the embodiments or the prior art. Obviously, for those of ordinary skill in the art, without creative efforts, other drawings can also be obtained based on these drawings.

[0018] Figure 1 Schematic diagram of the fixed-point number structure provided by the embodiment of the present invention; Figure 2 Schematic diagram of the lidar sensor and BRAM participating in the coordinate conversion process provided by the embodiment of the present invention; Figure 3 Architecture diagram of the optimized data cache processing architecture and the array of processing elements for the brute-force nearest neighbor search with a nearest neighbor threshold introduced by FPGA provided by the embodiment of the present invention; Figure 4The architecture diagram of the processing element array provided by the embodiments of the present invention; Figure 5 The flowchart of the improved ICP stage of the P2P-ICP algorithm provided by the embodiments of the present invention; Figure 6 The flowchart of calculating the Hessian matrix H(x) and constructing the Gauss-Newton equation based on the Hessian matrix provided by the embodiments of the present invention. Detailed implementation manners

[0019] To make the objectives, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. Apparently, the described embodiments are some but not all of the embodiments of the present invention. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the protection scope of the present invention.

[0020] It should be noted that in this document, the term "comprising", "including" or any other variant thereof is intended to cover a non-exclusive inclusion, such that a process, method, article or device comprising a series of elements includes not only those elements but also other elements not expressly listed, or further elements inherent to such process, method, article or device. Without further limitation, an element defined by the statement "comprising a..." does not exclude the existence of additional identical elements in the process, method, article or device comprising the element.

[0021] Embodiment 1 The present invention realizes a real-time lidar point cloud 3D mapping method implemented on an FPGA, so as to effectively deploy a 3D mapping algorithm based on brute-force nearest neighbor search into the FPGA. This application designs an efficient lidar point cloud coordinate conversion algorithm based on the parallel capabilities of the FPGA, constructs an energy-efficient data cache processing architecture based on the FPGA, reduces the computational imbalance and latency caused by lidar data preprocessing, constructs a parallel operation method for the distance between point cloud points in the brute-force nearest neighbor search algorithm with a nearest neighbor threshold introduced based on an array of processing elements, and reduces the complexity of the brute-force nearest neighbor search algorithm. The brute-force nearest neighbor search algorithm is improved by adding a nearest neighbor threshold. In order to improve the accuracy of point cloud matching, a mechanism based on the nearest neighbor threshold is established to solve the method of the minimum nearest neighbor value. When the matching is unsuccessful, the time doubling caused by multiple iterations is reduced. In addition, in order to improve the computational efficiency, a fixed-point number operation method for lidar data processing is proposed. When the lidar data is input, the original lidar data is quickly converted into coordinate data in the Cartesian coordinate system, and each frame of the point cloud is characterized by a quaternion (R, T). This method effectively solves the problem of wasting a large amount of storage resources and computational bandwidth by caching a large amount of iteratively converted data.

[0022] As Figure 1 shown, the real-time lidar point cloud 3D mapping method implemented on the FPGA provided by this application includes: The real-time lidar point cloud 3D mapping method implemented on the FPGA is executed using the Verilog language on a custom test board based on AMD's Kintex-7 FPGA chip. In order to adapt to the FPGA implementation, arithmetic calculation is a significant consideration. In order to optimize the lidar data processing time, this application selects fixed-point number operations, which are more resource-saving than floating-point numbers and can meet the computational accuracy requirements of many applications. The highest bit of the fixed-point number is used as the sign bit, and the rest represent integers and decimals, as Figure 1 shown.

[0023] Construct an efficient lidar point cloud coordinate conversion algorithm based on the parallel capabilities of the FPGA to achieve FPGA-accelerated processing of lidar data coordinate conversion: The software driver from the lidar sensor is rebuilt and moved into the chip processor system. The lidar data is output to the FPGA by the main data stream output protocol (MSOP) and the device information output protocol (DIFOP). The data provided by the main data stream output protocol contains point cloud data. The data transmitted by the device information output protocol contains the following calibration parameters: the horizontal error angle of the starting point of the lidar channel harness , representing the horizontal offset angle of the channel starting point relative to the central direction of the lidar; the horizontal installation deviation angle of the lidar channel ; the installation position offset of the lidar relative to the X-axis ; The installation position offset of the lidar relative to the Z-axis . These calibration parameters determine the data accuracy after error value compensation during the lidar data processing, but are fixed for each lidar. Therefore, when the system starts up, the FPGA calculates the relevant sine and cosine coefficients after obtaining the data transmitted by the device information output protocol, and stores the calibration parameters and sine-cosine parameters in the BRAM. During the subsequent coordinate conversion process, they are directly called from the BRAM to improve the subsequent calculation efficiency.

[0024] The coordinate conversion formula for lidar point cloud data is as follows: , , , where r is the distance from the lidar measurement target point to the lidar, is the pitch angle of the laser emitted by different channels of the lidar, is the azimuth angle of the lidar, which is the angle between the projection of the laser emitted by the lidar on the horizontal plane and the set reference direction. X, Y, and Z are the coordinates projected onto the Cartesian X-axis, Y-axis, and Z-axis.

[0025] To utilize the parallel computing power of the FPGA hardware and reduce memory utilization, through the expansion of trigonometric identities, the original equation for coordinate conversion of point cloud data is reformulated into a form suitable for parallel operation of the FPGA: ; ; ; Considering that the lidar rotates at a predetermined frequency and time period, and are fixed in each iteration. During system initialization, the coefficients of cosine and sine related to and are determined and stored in the BRAM for later use.

[0026] During the coordinate conversion process of lidar point cloud data in parallel by the FPGA, the following parameters are pre-read from the BRAM during system initialization: .

[0027] First, the FPGA uses the obtained parameters to calculate the following intermediate variables in parallel: ; , , , , , ; The FPGA then parallel-computes the following intermediate variables based on the intermediate variables obtained in the first-step operation : : , ; Finally, the FPGA parallel-computes the coordinates based on the intermediate variables of the first and second steps: , , .

[0028] The parallel computations in the three stages of coordinate transformation are processed and executed within three clock cycles of the FPGA, thereby making full use of the inherent parallel computing ability of the FPGA to efficiently perform coordinate transformation of lidar point cloud data, bringing higher efficiency to 3D mapping. And the arithmetic process of coordinate transformation implemented on the FPGA uses fixed-point arithmetic. Each multiplication step is calculated using combinational logic, while sequential logic is used for addition. The above method utilizes the inherent parallel processing ability of the FPGA to complete the coordinate transformation of lidar data within only three clock cycles. At a clock rate of 200 MHz, the operation duration is 15 ns. Compared with the CPU, the computing efficiency of this application is increased by at least one order of magnitude.

[0029] The FPGA optimizes the P2P-ICP (Peer-to-Peer Iterative Closest Point) algorithm for point cloud registration. The basic principle of the P2P-ICP algorithm is to find the point with the closest distance in the source point cloud (lidar point cloud) to each point in the target point cloud (map point cloud) through nearest neighbor search to form a corresponding relationship, and then use the least squares method to calculate the optimal rigid transformation matrix, including rotation and translation, and apply the calculated transformation to the source point cloud to achieve registration. This process is repeated until the stop condition is met. This application of the FPGA optimizes the P2P-ICP algorithm in the following aspects: First, to obtain the best results, this application uses brute-force nearest neighbor search with a nearest neighbor threshold as the nearest neighbor calculation method.

[0030] The process of brute-force nearest neighbor search with a nearest neighbor threshold includes: Set the scale of the source point cloud as N1 and the scale of the target point cloud as N2; set the nearest neighbor threshold, and the nearest neighbor threshold is not the minimum nearest neighbor value; Calculate the current point in the source point cloud Location coordinates With each point in the target point cloud Location coordinates The Euclidean distance between: ; if and The Euclidean distance between If the value of the current point in the source point cloud is greater than the given nearest neighbor threshold, and the midpoint of the target point cloud No match.

[0031] if and The Euclidean distance between If it is less than or equal to the given nearest neighbor threshold, further check and The Euclidean distance between Is it less than or equal to the midpoint of the target point cloud? With the current best matching point The Euclidean distance between them is: If yes, update the target point cloud midpoint The best matching point is , and update the Euclidean distance between it and the best matching point. If not, then the target point cloud midpoint The best matching point remains unchanged.

[0032] The above process is performed on all point cloud points in the source point cloud to find all the best matching point pairs.

[0033] During the specific implementation process, the following mechanism based on the nearest neighbor threshold is constructed to solve the minimum nearest neighbor value: the matching completion condition is that when the output meets the preset nearest neighbor threshold, the matching is completed only after continuing to search for the subsequent "set number" of point cloud data. Because the nearest neighbor threshold is not the minimum nearest neighbor value, based on the storage order, the subsequent points contain nearby point cloud points, and the subsequent point cloud points may produce a smaller nearest neighbor value. Using a smaller nearest neighbor value for registration can improve the registration quality, increase the accuracy of point cloud matching, and enhance the effect of 3D mapping. When the match is unsuccessful, the added nearest neighbor threshold reduces the time doubling caused by multiple iterations.

[0034] Optimization of data cache architecture for brute force nearest neighbor search with the introduction of nearest neighbor threshold: Figure 3As shown, the data stream used in the brute-force nearest neighbor search with a nearest neighbor threshold starts from the input of lidar data. After being corrected and preprocessed, the lidar data is converted into the Cartesian coordinate system and written into the DDR cache. During the writing process, the data stream is controlled to frame the data according to the requirements of the processing element array. The first frame is marked with "Key Frame Mark (KFMark)", and the first frame is given the initial rotation parameters. The other data frames in the cache are marked as "Data Mark". Subsequently, new "Key Frame Marks" are added according to the displacement Δt of the "Data Mark" frames participating in the matching relative to the key frame. In the optimized data cache architecture, the FPGA directly completes the low-latency parametric rotation through combinational logic when reading the point cloud coordinate data in the DDR cache, including: the rotation parameters are synchronized and aligned with the reading of the point cloud data in the DDR cache through the rotation parameter register, eliminating the computational dependence, and the rotation transformation is completed according to the rotation parameters within the same clock cycle when the point cloud data is transmitted, without caching.

[0035] In this application, the FPGA configures a processing element array for parallel computing the distance between point pairs in the brute-force nearest neighbor search with a nearest neighbor threshold. To improve the computing efficiency, the processing element array of the FPGA preloads the map point cloud data and real-time point cloud data required for the brute-force nearest neighbor search with a nearest neighbor threshold, and at the same time extracts the point cloud data sequentially for parallel computing of the Euclidean distance. The processing element array queries the local map point cloud data cache. Subsequently, the cache address data marked as "Data Mark" or "Key Frame Mark" is extracted and input into the processing element array. As Figure 3 and Figure 4 shown, the processing element array extracts k points from the source point cloud sequentially each time: , the processing element array extracts i points from the target point cloud sequentially each time: ; the processing element array calculates the Euclidean distance between point pairs in each clock cycle. After calculating the Euclidean distance, point pair matching is performed according to the brute-force nearest neighbor search with a nearest neighbor threshold. The calculated matching point pairs are provided to the registration module, and the registration module outputs the pose parameters according to the positions of the paired source point cloud and target point cloud. The pose includes the rotation matrix R and the translation matrix t.

[0036] When the displacement Δt of the "data marker" participating in the matching relative to the "key marker" frame exceeds the "set number", the currently calculated "data marker" frame will be marked as a key frame marker. This process is fed back to the marker management implementation of the DRAM. During the matching process, when the displacement Δt relative to the key frame does not reach the set number, the system will clear the current data and buffer information, but will retain the current pose parameters: the rotation matrix R and the translation vector t, as the iterative initial parameters for subsequent data frames. The current pose parameters are stored in the local map data: when the number of accumulated key frames exceeds the'maximum number of key frames', the earliest key frame is transmitted to the host computer via Ethernet in chronological order, and its pose parameters are output for subsequent 3D mapping use.

[0037] By applying an array of processing elements, the computational complexity of brute-force nearest neighbor search with a nearest neighbor threshold is reduced to , where N represents the scale of the point cloud, and a mechanism based on the nearest neighbor threshold is established to solve the minimum nearest neighbor value, which can significantly improve the computational efficiency while maintaining high-precision registration, and is suitable for real-time systems sensitive to latency.

[0038] To estimate the position of the mobile platform, the P2P-ICP algorithm selected in this application uses brute-force nearest neighbor search with a nearest neighbor threshold for point cloud alignment to generate pose parameters, and then uses the generated pose parameters as pose prior estimates, and finally obtains the final pose through iteration of the Gauss-Newton algorithm. The improvement of the ICP stage of the P2P-ICP algorithm in this application is as follows Figure 5 shown, including: S10, using the generated pose parameters as pose prior estimates: .

[0039] S20, setting the pose parameters at any iteration step ; S30, at the pose parameters at any iteration step , using brute-force nearest neighbor search with a nearest neighbor threshold to determine the paired source point cloud points and target point cloud points: and ; S40, calculating the Hessian matrix H(x) and constructing the Gauss-Newton equation based on the Hessian matrix, as shown in Figure 6 shown, including: S41, constructing an error function for point-to-point alignment: ; S42, calculating the derivative of the error function with respect to the rotation matrix by right-multiplying the perturbation model: In the right-multiplying perturbation model, the error function after applying the rotation perturbation is ; Among them, the perturbation of the rotation matrix is obtained by the first-order approximation of the Taylor expansion of the exponential function , so , is the three-dimensional rotation perturbation vector, and the skew-symmetric vector representation of the rotation perturbation vector is: .

[0040] The error function is transformed into: ; Also, due to the properties of the skew-symmetric function:

[0041] Among them, is the skew-symmetric matrix of: .

[0042] Then the error function after applying the rotation perturbation is ; The derivative of the error function after applying the rotation perturbation with respect to the perturbation is: .

[0043] S43, calculate the derivative of the error function with respect to the translation matrix as: ; S44, calculate the Hessian matrix H(x) of the derivatives of the error function with respect to the rotation matrix and the translation matrix, and construct the Gauss-Newton equation: Use the derivatives of the error function with respect to the rotation matrix and the translation matrix to construct the Jacobian matrix: ; Then calculate the Hessian matrix through the Jacobian matrix: ; The gradient vector of the error function is: ; The Gauss-Newton equation is: .

[0044] S50, if the rank of the Hessian matrix H(x) is 6, then go to S60, otherwise the iteration ends.

[0045] S60, when the rank of the Hessian matrix H(x) is 6, the decomposition result is unique, decompose the Hessian matrix H(x) and solve the Gauss-Newton equation: The of the Hessian matrix H(x)The decomposed form is: , where L is a unit lower triangular matrix (diagonal elements are 1 and upper triangular elements are all 0), and D is a diagonal matrix.

[0046] The decomposition process is as follows: Initialization: , that is directly take the first diagonal element of the Hessian matrix H(x). , The first column of is the off - diagonal elements of the first column of the Hessian matrix H(x) divided by the first diagonal element.

[0047] Recursive calculation and , w = 1, …, 5, i = w + 1, …, 5: , that is the current diagonal element subtract the accumulation of all previous . , that is the current off - diagonal element subtract the accumulation of all previous , and then divide by .

[0048] After decomposition, the Gauss - Newton equation becomes: , Let , Then it is solved in three steps: (1) Forward substitution to solve : Since is a unit lower triangular matrix, its form is:

[0049] The solution of the equation is:

[0050] (2) Diagonal scaling to solve : Since is a diagonal matrix, directly divide element by element: } ; (3) Backward substitution to solve : Since is a unit upper triangular matrix, its form is: ; Equation The solution of the equation is: , calculating in reverse order from the last line, using the already obtained recursion .

[0051] S70, if the solution of the Gauss-Newton equation then go to S80, otherwise go to S30, S80, according to Update the attitude parameters: The rotation and translation increments are respectively: ; Use the Rodriguez rotation formula to obtain the updated rotation matrix according to the rotation increment : ; ∧ is the symbol of the skew-symmetric matrix.

[0052] Then the attitude parameters are updated to: .

[0053] In the embodiments provided by the present invention, it should be understood that the disclosed structures and methods can be implemented in other ways. For example, the structural embodiments described above are only illustrative. For example, the division of the units is only a logical function division. In actual implementation, there may be other division methods. For example, multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point, the displayed or discussed couplings or direct couplings or communication connections between each other can be through some interfaces, and the indirect couplings or communication connections of the structures or units can be in electrical, mechanical or other forms.

[0054] The units described as separate components may or may not be physically separated. The components displayed as units may or may not be physical units, that is, they can be located in one place, or they can be distributed to multiple network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0055] In addition, in each embodiment of the present invention, the functional units can be integrated in a processing unit, or each unit can exist physically alone, or two or more units can be integrated in one unit. The above integrated units can be implemented in the form of hardware or in the form of software functional units.

[0056] The above are only specific embodiments of the present invention, enabling those skilled in the art to understand or implement the present invention. Various modifications to these embodiments will be obvious to those skilled in the art, and the general principles defined herein can be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention will not be limited to these embodiments shown herein, but rather will conform to the broadest scope consistent with the principles and novel features claimed herein.

Claims

1. A real-time lidar point cloud 3D mapping method implemented on an FPGA, characterized in that Including: Construct a lidar point cloud coordinate conversion algorithm based on the parallel capabilities of FPGA to achieve FPGA-accelerated processing of lidar data coordinate conversion; Use brute-force nearest neighbor search with a nearest neighbor threshold introduced as the nearest neighbor calculation method; FPGA optimizes the data cache architecture for brute-force nearest neighbor search with a nearest neighbor threshold introduced, configures an array of processing elements for parallel calculation of distances between point pairs, and accelerates brute-force nearest neighbor search in hardware; FPGA executes the P2P-ICP algorithm to perform point cloud alignment using brute-force nearest neighbor search with a nearest neighbor threshold introduced to generate pose parameters, and then uses the generated pose parameters as a pose prior estimate to perform Gaussian-Newton algorithm iteration through an improved ICP stage to obtain the final pose; FPGA adopts a fixed-point number operation method for lidar data processing.

2. The real-time lidar point cloud 3D mapping method implemented on an FPGA according to claim 1, wherein The lidar point cloud coordinate conversion algorithm based on the parallel capabilities of FPGA includes: After the FPGA obtains the calibration parameters transmitted by the device information output protocol at system startup, it calculates the sine and cosine coefficients related to the calibration parameters and the pitch and azimuth angles of the lidar, and stores the calibration parameters and the sine and cosine coefficients in the BRAM; the calibration parameters include: the horizontal error angle of the starting point of the lidar channel wire harness , the horizontal installation deviation angle of the lidar channel , the installation position offset of the lidar relative to the X-axis , the installation position offset of the lidar relative to the Z-axis ; During the parallel coordinate conversion of lidar point cloud data by FPGA, pre-stored parameters are pre-read from BRAM during system initialization: ； First, FPGA uses the obtained parameters to parallel calculate the following intermediate variables: ； ， ， ， ， ， ； The FPGA then performs parallel operations based on the intermediate variables obtained in the first step Parallelly calculate the following intermediate variables : ， ； Finally, FPGA parallel calculates the coordinates based on the intermediate variables in the first and second steps: ， ， 。 3. The real-time lidar point cloud 3D mapping method implemented on the FPGA according to claim 1, wherein Brute-force nearest neighbor search with a nearest neighbor threshold introduced includes: Calculate the Euclidean distance between the position coordinates of the current point in the source point cloud and the position coordinates of each point in the target point cloud ; ​ If and the Euclidean distance is greater than a given nearest neighbor threshold, then the current point in the source point cloud does not match the point If and the Euclidean distance is less than or equal to the given nearest neighbor threshold, then further check and the Euclidean distance is whether less than or equal to the Euclidean distance between the point in the target point cloud and the current best matching point. If so, then update the best matching point of the point in the target point cloud to , and update the Euclidean distance between it and the best matching point. If not, then the best matching point of the point in the target point cloud remains unchanged; Among them, the matching completion condition is that when the output meets the preset nearest neighbor threshold, continue to search for the subsequent "set number" of point cloud data before the matching is completed; Execute the above process for all point cloud points in the source point cloud to find all the best matching point pairs.

4. The real-time lidar point cloud 3D mapping method implemented on an FPGA according to claim 1, wherein During the process of writing point cloud data to DDR, optimize the data cache architecture to control the data flow to perform data framing according to the requirements of the processing element array. The first frame is marked with a "key frame mark", and the first frame is given an initial rotation parameter; other cached data frames are marked as "data marks"; subsequently, new "key frame marks" are added according to the displacement △t of the "data mark" frames participating in the matching relative to the key frame; FPGA directly completes low-latency parametric rotation through combinational logic when reading out point cloud coordinate data in the DDR cache.

5. The real-time lidar point cloud 3D mapping method implemented on an FPGA according to claim 1, wherein The processing element array of PGA pre-loads the map point cloud data and real-time point cloud data required for brute-force nearest neighbor search with a nearest neighbor threshold introduced, and simultaneously extracts point cloud data in sequence for parallel calculation of Euclidean distances; after calculating the Euclidean distances, perform point pair matching according to brute-force nearest neighbor search with a nearest neighbor threshold introduced; provide the calculated matching point pairs to the registration module, and the registration module outputs pose parameters according to the positions of the paired source point cloud and target point cloud, where the pose includes a rotation matrix R and a translation matrix t.

6. The real-time lidar point cloud 3D mapping method implemented on an FPGA according to claim 5, characterized in that When the displacement △t relative to the key frame exceeds the "set number", the current calculated data mark frame will be marked as a key frame mark; during the matching process, when the displacement △t relative to the key frame does not reach the set number, the system will clear the current data and buffer information, but will retain the current pose parameters as the iterative initial parameters for subsequent data frames; when the cumulative number of key frames exceeds the'maximum number of key frames', the earliest key frame will be transmitted to the host computer through Ethernet in chronological order, and at the same time, its pose parameters will be output for subsequent 3D mapping.

7. The real-time lidar point cloud 3D mapping method implemented on the FPGA according to claim 1, characterized in that, The improved ICP stage includes: S10, using the generated attitude parameters as the prior attitude estimate: ; S20, set the attitude parameters of any iteration step ; S30, the pose parameters at any iteration step , use brute-force nearest neighbor search with a introduced nearest neighbor threshold to determine the paired source point cloud points and target point cloud points: and ; S40. Calculate the Hessian matrix H(x) and construct the Gauss-Newton equation based on the Hessian matrix; S50. If the rank of the Hessian matrix H(x) is 6, go to S60; otherwise, the iteration ends; S60, decompose the Hessian matrix H(x) and solve the Gauss-Newton equation; ​ S70, if the solution of the Gauss-Newton equation then go to S80, otherwise go to S30 S80, according to Update the attitude parameters.

8. The real-time lidar point cloud 3D mapping method implemented on an FPGA according to claim 7, wherein The process of calculating the Hessian matrix H(x) and constructing the Gauss-Newton equation based on the Hessian matrix includes: S41. Construct an error function for point-to-point alignment: ； S42, calculate the derivative of the error function with respect to the rotation matrix by right-multiplying the perturbation model : ； S43, calculate the derivative of the error function with respect to : ； S44. Calculate the Hessian matrix H(x) and construct the Gauss-Newton equation: Construct a Jacobian matrix using the derivatives of the error function with respect to the rotation matrix and the translation matrix: ； Then the Hessian matrix is as follows: ; The gradient vector of the error function is as follows: ; The Gauss-Newton equation is as follows: .

9. The real-time lidar point cloud 3D mapping method implemented on an FPGA according to claim 7, wherein Performing the decomposition of the Hessian matrix H(x) includes: The decomposition form of the Hessian matrix H(x) is: ， where L is a unit lower triangular matrix and D is a diagonal matrix; The decomposition process is as follows: Initialization: That is directly take the first diagonal element of the Hessian matrix H(x); , the first column of is the off-diagonal elements of the first column of the Hessian matrix H(x) divided by the first diagonal element; Recursive calculation and , w = 1, …, 5, i = w + 1, …, 5: , i.e., the current diagonal element minus the sum of all previous accumulations; , that is, the current non-diagonal element minus the sum of all previous , and then divided by .

10. The real-time lidar point cloud 3D mapping method implemented on an FPGA according to claim 9, wherein, After decomposition, the Gauss-Newton equation becomes: ， Let , , and solve it in the following three steps: Forward substitution solution : Since is a unit lower triangular matrix, and its form is: The solution is: ； Diagonal scaling solution : Since is a diagonal matrix, directly perform element-by-element division: ； Backward substitution solution : Since is an identity upper triangular matrix and has the form: ； Equation The solution is: , calculate in reverse order from the last line, using the already obtained recursion .

Images