Correlation scan matching hardware circuit and heterogeneous computing architecture for TSDF maps

Abstract

The present invention discloses a correlation scanning matching hardware circuit and a heterogeneous computing architecture of a TSDF map, wherein the correlation scanning matching hardware circuit includes a memory module, a point cloud processing module, a TSDF value calculation module, a state machine control module and an interconnection bus, wherein the memory module, the point cloud processing module, the TSDF value calculation module and the state machine control module all establish a data transmission relationship through the interconnection bus; the state machine control module is used to schedule the working states of the memory module, the point cloud processing module and the TSDF value calculation module, so that each time the point cloud processing module executes the coordinate system transformation and the rotation transformation once, the TSDF value calculation module executes the division operation once to obtain a TSDF value corresponding to a posture searched in the search window, until the TSDF value calculation module counts that all postures in the search window have been searched.

Description

Technical Field

[0001] The present invention relates to the technical field of hardware acceleration, and in particular to a correlation scan matching hardware circuit and a heterogeneous computing architecture based on a TSDF map. Background Art

[0002] A scene with dimension d can be represented using voxels of the same size, also of dimension d. For example, in a 3D scene, a voxel grid is a data structure that uses fixed-size cubes as the smallest unit to represent a 3D object. For a 2D scene, a voxel is a 2D grid, with the position of a voxel defined by its center coordinates. Voxels can be thought of as points in a point cloud map.

[0003] When using the signed distance function sdf(x) for scene reconstruction, each voxel contains two values: the signed distance function value and the weight value. The signed distance function value refers to the signed distance from the voxel center to the nearest neighboring object surface in the real scene, that is, it represents the relative distance between the voxel center and the nearest object surface in the current measurement direction. When the voxel center is before the object surface (not inside the object), the signed distance function value is defined as a positive number; when the voxel center is behind the object surface (inside the object) or on the side blocked by the object surface, the signed distance function value is defined as a negative number. The weight value contained in the voxel is used to evaluate the measurement uncertainty of the signed distance function value, that is, it represents the expected uncertainty of the corresponding signed distance function value of the voxel in the current measurement direction (on the line of sight of the signed distance from the voxel center to the surface).

[0004] When the signed distance function sdf(x) is truncated at a preset cutoff distance t, a truncated signed distance function tsdf(x) is defined as the truncation of the signed distance function sdf(x). This is equivalent to mapping the signed distance function sdf(x) into a truncated signed distance field. For example, in a three-dimensional space of a specific size, the obstacle cube to be measured is subdivided into a small three-dimensional voxel grid, and each voxel stores its quantized distance to the real scene surface through the truncated signed distance function tsdf(x) (obtained by mapping the signed distance function value within the range of ±t of the truncation distance according to a preset encoding method).

[0005] Currently represents TSDF values ​​as tsdf iThe integral of (x) can be calculated by weighted summation or weighted incremental update, where i is the number of currently stored point clouds, which is also the number of voxels. The TSDF value can be regarded as the point cloud corresponding to the laser data frame mapped to the voxel through the signed distance function calculation. The truncated signed distance function tsdf(x) contained in each voxel and its matching weight value are used as the coordinate value of the voxel in the global coordinate system to construct a two-dimensional grid map based on TSDF (hereinafter referred to as TSDF map). Especially in the field of mobile robot SLAM, TSDF map is more suitable for integration into SLAM algorithm than occupancy grid map to perform robot map navigation and positioning operations. However, the constructed TSDF map has a significant drawback: compared with occupancy grid map, TSDF map will consume more storage space.

[0006] Moreover, one of the most challenging research topics in LiDAR-based mobile robot SLAM algorithms is the use of scan matching algorithms to achieve precise robot positioning. The Correlation Scan Matching (CSM) algorithm is one of the most widely used scan matching algorithms. Its advantages lie in its moderate computational cost and small cumulative error. The Correlation Scan Matching (CSM) algorithm is often optimized based on a priori estimates. Within a search window of limited size, the currently collected radar point cloud is transformed into a map through rotation and translation to complete the operation. However, the CSM algorithm has a significant drawback: its extremely high computational complexity makes it difficult to deploy on embedded computing platforms with limited computing power. Summary of the Invention

[0007] In order to overcome the problem that the TSDF map disclosed in the prior art and the correlation scan matching algorithm used on the map consume a large amount of memory space, the present invention discloses a correlation scan matching hardware circuit and heterogeneous computing architecture based on the TSDF map. The specific technical solutions are as follows:

[0008] The correlation scan matching hardware circuit of the TSDF map is electrically connected to the laser radar; the correlation scan matching hardware circuit includes a memory module, a point cloud processing module, a TSDF value calculation module, a state machine control module and an interconnection bus, wherein the memory module, the point cloud processing module, the TSDF value calculation module and the state machine control module all establish a data transmission relationship through the interconnection bus; the point cloud processing module is used to read the currently stored point cloud from the memory module under the control of the state machine control module, and then control the read point cloud to perform coordinate system transformation, and set the result of the coordinate system transformation to a discrete point cloud, so as to realize the alignment of the currently stored point cloud to the coordinate system of the pre-expanded raster map. and then storing the result of the coordinate system transformation into the memory module; the point cloud processing module is further used to read the currently stored point cloud from the memory module under the control of the state machine control module, and then control the read point cloud to perform a rotation transformation, and then update the result of the rotation transformation to the currently stored point cloud stored in the memory module, and then control the updated point cloud to perform the aforementioned coordinate system transformation to determine the completion of a matching process of the correlation scan matching of the map; wherein each matching process of the correlation scan matching of the map includes a rotation transformation and a coordinate system transformation; the TSDF value calculation module is used to read the discrete point cloud from the memory module under the control of the state machine control module, and then calculate according to the preset coordinate offset value and search step size. Calculate the index value of the discrete point cloud mapped to the storage space of the truncated signed distance function value, and set the index value to the read address of the truncated signed distance function value and the read address of the weight value stored in the memory module; the truncated signed distance function value and the weight value have a matching index value; the search step is the number of grid searches that need to be traversed each time a posture is searched within the same search window; the search window is a neighborhood grid area centered on a point cloud within the pre-expanded grid map; the TSDF value calculation module is also used to read the truncated signed distance function value and its matching weight value from the memory module according to the currently set read address under the control of the state machine control module, and then control the TSDF value calculation module The built-in divider of the block performs a division operation to equivalently complete the weighted average processing of the truncated signed distance function value mapped by the currently stored point cloud, and sets the result of each operation output of the divider as the TSDF value, which is used to represent the positioning accuracy of a posture searched in the search window; wherein, each time the point cloud processing module completes a matching process of the correlation scan matching of the map, the built-in divider of the TSDF value calculation module performs a division operation to obtain a TSDF value corresponding to a new posture in the search window; the memory module is used to store the currently stored point cloud, the discrete point cloud obtained by the transformation of the currently stored point cloud by the point cloud processing module, and the pre-set truncated signed distance function and its matching weight value;Each type of data is stored in a different block storage space of the same memory module.

[0009] This technical solution calls the TSDF value calculation module to perform a division operation for each posture within the search window in the pre-expanded grid map to obtain the corresponding TSDF value at the posture, and the division operation is scheduled by the state machine control module based on the discrete point cloud provided by the point cloud processing module and the rotation transformation results for updating the discrete point cloud provided by the point cloud processing module, while ensuring that the associated operation results and transformation results are shared on the interconnection bus; compared with the existing technology, the hardware operation method of this technical solution reduces the amount of division operations in the hardware, does not increase the additional storage space of the hardware, and reduces the consumption of a large amount of programmable logic resources.

[0010] Furthermore, the TSDF value calculation module includes a selector, a frequency division counter, a pre-calculation unit, a divider, a first accumulator, a second accumulator, and a third accumulator; the TSDF value calculation module is used to, under the control of the state machine control module, send the truncated signed distance function value into the first accumulator, and send the weight value into the second accumulator, and send the truncated signed distance function value and the weight value matched therewith into the third accumulator, and control each accumulator to perform an accumulation operation simultaneously; wherein the input ends of the three accumulators are configured to receive data transmitted sequentially by the interconnection bus, and the third accumulator is used to output the cumulative sum of the products of the truncated signed distance function value and the weight value matched therewith; The output terminals of the three accumulators are respectively connected to the corresponding data input terminals of the selector, the selection input terminal of the selector is connected to the counting output terminal of the frequency division counter, and the selector is provided with a data output terminal matching each data input terminal, which is used to select and output the data received by the data input terminal of the selector under the counting triggering action of the frequency division counter to adapt to the enabling timing of the divider; each data output terminal of the selector is respectively connected to the corresponding input terminal of the pre-calculation unit; the pre-calculation unit includes a numerator adder, a first numerator subtractor, a second numerator subtractor, a numerator shift register, a first numerator subtractor and a denominator shift register; the numerator adder belongs to the adder, the first numerator subtractor, the second numerator subtractor The numerator and denominator shift registers are all subtractors, and the numerator shift register and the denominator shift register are all shift registers; wherein, the first input end of the second numerator subtractor is connected to the output end of the first accumulator, and the second input end of the second numerator subtractor is connected to the output end of the third accumulator; the second numerator subtractor is used to control the subtraction of the accumulated sum output by the first accumulator and the accumulated sum output by the third accumulator, and then output the difference between the two subtracted data; the first numerator subtractor is provided with an input end for receiving the number value of the currently stored point cloud transmitted by the interconnection bus; the first numerator subtractor is provided with another input end connected to the output end of the second accumulator; the first numerator subtractor is used to control the accumulated sum output by the second accumulator and the difference between the two subtracted data; The number values ​​of the currently stored point cloud transmitted by the bus are subtracted, and then the difference between the two subtracted data is output; the input end of the molecular shift register is connected to the output end of the first molecular subtractor, and the molecular shift register is used to shift the difference output by the first molecular subtractor so that the binary digit width obtained by the difference output by the first molecular subtractor after the shift processing is equal to the bit width of the accumulated sum output by the first accumulator; the two input ends of the molecular adder are respectively connected to the output end of the molecular shift register and the output end of the second molecular subtractor, and the molecular adder is used to add the shift output result of the molecular shift register to the difference output by the second molecular subtractor, and then configure the sum of the two added data as the dividend;The first numerator subtractor is used to control the subtraction of the accumulated sum output by the second accumulator and the number value of the currently stored point cloud transmitted by the interconnection bus, and then output the difference between the two subtracted data; the input end of the denominator shift register is connected to the output end of the first numerator subtractor, and the denominator shift register is used to shift the difference output by the first numerator subtractor so that the binary digit width obtained by the shift processing of the difference output by the first numerator subtractor is one bit lower than the binary digit width of the accumulated sum output by the first accumulator; The two input terminals of the divider are respectively connected to the output terminal of the numerator adder and the output terminal of the denominator shift register. The divider is used to, when enabled, control the dividend output by the numerator adder and the divisor output by the denominator shift register to perform a division operation, then send the calculated quotient back to the interconnection bus, determine that the quotient becomes the TSDF value corresponding to a pose within the search window, and determine that the divider, by performing a single division operation, equivalently completes a weighted average processing of the truncated signed distance function values ​​mapped from the currently stored point cloud.

[0011] This technical solution equates the weighted average processing of the truncated signed distance function value to an integral operation with the truncated distance as the integral variable and the number of currently stored point clouds as the integral upper limit. Therefore, this technical solution first merges the truncated signed distance function value, the weight, and the product of the truncated signed distance function value and the weight in hardware, then performs parallel accumulation processing on these similar terms, selects the accumulated term of the weight as the frequency division parameter of the divider, and then performs a division operation based on the shift processing of both the dividend and the divisor. Under the counting trigger of the frequency division counter, all the truncated signed distance function values ​​and weight values ​​corresponding to the current iteration state of the correlation scan matching algorithm in the point cloud processing module can be pushed into the TSDF value calculation module to perform a division operation. This saves the computing resources of the divider.

[0012] Furthermore, the TSDF value calculation module includes a grid index submodule, and the grid index submodule includes a first index subtractor, a second index subtractor, a first index adder, a second index adder, a third index adder and an index multiplier; wherein, the first index subtractor and the second index subtractor are both subtractors, and the first index adder and the second index adder are both adders; the first input end of the first index subtractor is used to receive the horizontal coordinate of the discrete point cloud transmitted from the interconnection bus, wherein the horizontal coordinate of the discrete point cloud transmitted to the first index subtractor by the interconnection bus is derived from the memory module; the second input end of the first index subtractor is used to receive the horizontal axis coordinate offset value transmitted from the interconnection bus , wherein the preset coordinate offset value includes a horizontal axis coordinate offset value, and the horizontal axis coordinate offset value transmitted by the interconnection bus is derived from the preset coordinate offset value stored in the map offset value register; the map offset value register is a parameter register set inside the correlation scan matching hardware circuit, and is used to store the coordinate offset value associated with the pre-expanded grid map; the first input end of the first index adder is connected to the output end of the first index subtractor; the second input end of the first index adder is used to receive the horizontal axis coordinate search step transmitted by the interconnection bus; wherein the search step includes the horizontal axis coordinate search step, and the horizontal axis coordinate search step transmitted by the interconnection bus is derived from the search window parameter register The search step stored in the device, the search window parameter register is a parameter register set inside the correlation scan matching hardware circuit, which is used to store the posture covered by the search window and the associated search information; the sum value output by the first index adder is configured as the horizontal axis direction index value in the storage space where the discrete point cloud is mapped to the truncated signed distance function value; the first input end of the second index subtractor is used to receive the vertical coordinate of the discrete point cloud transmitted by the interconnection bus, wherein the vertical coordinate of the discrete point cloud transmitted to the second index subtractor by the interconnection bus is derived from the memory module; the second input end of the second index subtractor is used to receive the vertical axis coordinate offset value transmitted by the interconnection bus, wherein the pre Assume that the coordinate offset value also includes a vertical axis coordinate offset value, and the vertical axis coordinate offset value transmitted by the interconnection bus is also derived from the preset coordinate offset value stored in the map offset value register; the first input end of the second index adder is connected to the output end of the second index subtractor; the second input end of the second index adder is used to receive the vertical axis coordinate search step transmitted by the interconnection bus; wherein, the search step also includes a vertical axis coordinate search step, and the vertical axis coordinate search step transmitted by the interconnection bus is also derived from the search step stored in the search window parameter register; the sum value output by the second index adder is configured as the vertical axis direction index value in the storage space where the discrete point cloud is mapped to the truncated signed distance function value.

[0013] This technical solution uses two identical addition and subtraction operation combination structures to respectively calculate the horizontal axis direction index value in the storage space mapped to the truncated signed distance function value, and the vertical axis direction index value in the storage space mapped to the truncated signed distance function value, thereby realizing the parallel conversion of the horizontal and vertical coordinates of the discrete point cloud into discrete index information in the storage space of the truncated signed distance function value.

[0014] Furthermore, the grid index submodule also includes a third index adder and an index multiplier, the third index adder belongs to the adder, and the index multiplier belongs to the multiplier; the first input end of the index multiplier is connected to the output end of the second index adder; the second input end of the index multiplier is used to receive the number of row grids transmitted from the interconnection bus, wherein the number of row grids is the number of grids that each row of the storage space of the truncated signed distance function value can occupy, which is stored in the map size register and transmitted to the interconnection bus by the map size register; the map size register is a parameter register set inside the correlation scan matching hardware circuit , used to store the size range of the pre-expanded grid map and the associated expansion information; the first input end of the third index adder is connected to the output end of the first index adder, and the second input end of the third index adder is connected to the output end of the index multiplier. The third index adder is used to control the product of the number of row grids and the sum value output by the second index adder and add the horizontal axis index value, and then set the sum value obtained by the addition as the index value in the storage space of the discrete point cloud mapped to the truncated signed distance function value, and send it to the interconnection bus, so as to read the truncated signed distance function value and the weight value matching the index value from the memory module.

[0015] In this technical solution, an index multiplier controls the multiplication of the number of row grids and the vertical axis index value, and then a third index adder controls the addition of the horizontal axis index value and the product output by the index multiplier. The index value of the discrete point cloud mapped to the storage space of the truncated signed distance function value is calculated in a row scanning query manner as the index value or address of the linear storage space, so that the interconnection bus can read the truncated signed distance function value and the weight value matching the index value from the memory module.

[0016] Furthermore, the grid index submodule also includes a third index adder and an index multiplier, the third index adder is an adder, and the index multiplier is a multiplier; the first input end of the index multiplier is connected to the output end of the first index adder; the second input end of the index multiplier is used to receive the number of column grids transmitted from the interconnection bus; wherein the number of column grids is the number of grids that each column of the storage space of the truncated signed distance function value can occupy, which is stored in the map size register and transmitted to the interconnection bus by the map size register; the map size register is a parameter register set inside the correlation scan matching hardware circuit, Used to store the size range of the pre-expanded grid map and the associated expansion information; the first input end of the third index adder is connected to the output end of the second index adder, and the second input end of the third index adder is connected to the output end of the index multiplier; the third index adder is used to control the product of the number of column grids and the sum value output by the first index adder and add the vertical axis index value, and then set the sum value obtained by the addition as the index value in the storage space of the discrete point cloud mapped to the truncated signed distance function value, and send it to the interconnection bus, so as to facilitate reading the truncated signed distance function value and the weight value matching the index value from the memory module.

[0017] In this technical solution, an index multiplier controls the multiplication of the number of column grids and the horizontal axis index value, and then a third index adder controls the addition of the vertical axis index value and the product output by the index multiplier. The index value of the discrete point cloud mapped to the storage space of the truncated signed distance function value is calculated in a column scanning query manner as the index value or address of the linear storage space, so that the interconnection bus can read the truncated signed distance function value and the weight value matching the index value from the memory module.

[0018] Furthermore, the pre-expanded grid map is a grid map obtained by expanding and transforming the original point cloud-defined map in accordance with the expansion parameters required for the maximum detection radius reached by the currently stored point cloud, the expansion parameters required for executing the coordinate system transformation, the expansion parameters required for executing the rotation transformation, and the expansion parameters required for aligning the bus bit width. The original point cloud is all the laser point clouds obtained by the current scan of the laser radar. The map defined by the original point cloud is a grid map centered on the laser radar and defined with the maximum detection diameter reached in the original point cloud as the side length of the map. The alignment bus bit width means that the memory of the grid covered by the side length of the pre-expanded grid map is equal to the bit width of the bus transmitting the grid map. The preset coordinate offset value is determined by the sum of the coordinate translation parameters required for the maximum detection radius reached by the currently stored point cloud, the coordinate translation parameters required for executing the coordinate system transformation, the coordinate translation parameters required for executing the rotation transformation, and the coordinate translation parameters required for aligning the bus bit width. The pre-expanded grid map is based on the maximum coverage radius of the point cloud, and is expanded in sequence according to the coordinate translation parameters, coordinate rotation parameters, and memory parameters occupied by the map side length. This can not only constrain the actual usage size of the map, but also adapt to the bus bit width used during map transmission, ensure data alignment, and reduce the storage space occupied by the TSDF map.

[0019] Furthermore, the point cloud processing module includes a point cloud discretization submodule; the point cloud discretization submodule includes a first discrete adder, a first discrete subtractor, a first discrete multiplier, a second discrete adder, a second discrete subtractor and a second discrete multiplier, the first discrete adder and the second discrete adder are both adders, the first discrete subtractor and the second discrete subtractor are both subtractors, and the first discrete multiplier and the second discrete multiplier are both multipliers; the first input end of the first discrete adder is used to receive the horizontal coordinate of the point cloud transmitted by the interconnection bus, wherein the point cloud transmitted to the first discrete adder by the interconnection bus is derived from the currently stored point cloud stored in the memory module; the horizontal coordinate of the point cloud belongs to the horizontal coordinate value in the laser coordinate system. ; The second input end of the first discrete adder is used to receive the horizontal coordinate of the robot position transmitted from the interconnection bus, wherein the horizontal coordinate of the robot position is a pre-calculated horizontal axis coordinate value of the robot in the world coordinate system; the first discrete adder is used to control the addition of the horizontal coordinate of the point cloud and the horizontal coordinate of the robot position, and then output the sum obtained by the addition, so that the sum becomes the horizontal axis coordinate value of the point cloud converted to the world coordinate system; the first input end of the first discrete subtractor is connected to the output end of the first discrete adder; the second input end of the first discrete subtractor is used to receive the maximum horizontal coordinate value of the map transmitted from the interconnection bus, and the maximum horizontal coordinate value of the map is derived from the map size register and transmitted to the interconnection bus by the map size register ; Wherein, the map size register is a parameter register set inside the correlation scan matching hardware circuit, which is used to store the size range of the grid map that meets the transmission requirements of the bus width; the difference output by the first discrete subtractor becomes the vertical coordinate value of the grid map that meets the transmission requirements of the bus width; the first input end of the first discrete multiplier is connected to the output end of the first discrete subtractor; the second input end of the first discrete multiplier is used to receive the inverse of the map resolution transmitted from the interconnection bus, and the inverse of the map resolution is derived from the map resolution register and transmitted to the interconnection bus by the map resolution register; wherein, the map resolution register is a parameter register set inside the correlation scan matching hardware circuit, which is used to store The first discrete multiplier is configured to store the resolution information of the grid map that meets the transmission requirements of the bus bit width; the product value output by the first discrete multiplier is configured as the horizontal coordinate value of the discrete point cloud to complete the coordinate system transformation, so as to align the horizontal coordinate value of the currently stored point cloud to the coordinate system of the grid map that meets the transmission requirements of the bus bit width; the first discrete multiplier is further used to output the product value obtained by multiplication to the memory module through the interconnection bus, so that the TSDF value calculation module can read the horizontal coordinate value of the discrete point cloud; the first input end of the second discrete adder is used to receive the vertical coordinate of the point cloud transmitted from the interconnection bus, wherein the point cloud transmitted to the second discrete adder by the interconnection bus is derived from the currently stored point cloud stored in the memory module;The vertical coordinate of the point cloud belongs to the vertical coordinate value in the laser coordinate system; the second input end of the second discrete adder is used to receive the vertical coordinate of the robot position transmitted by the interconnection bus, wherein the vertical coordinate of the robot position is the pre-calculated vertical axis coordinate value of the robot in the world coordinate system; the second discrete adder is used to control the addition of the vertical coordinate of the point cloud and the vertical coordinate of the robot position, and then output the sum obtained by the addition, so that the sum becomes the vertical axis coordinate value of the point cloud converted to the world coordinate system; the first input end of the second discrete subtractor is connected to the output of the second discrete adder; the second input end of the second discrete subtractor is used to receive the maximum vertical coordinate value of the map transmitted by the interconnection bus, and the maximum vertical coordinate value of the map is derived from the map size register and is obtained by the map size register. The size register is transmitted to the interconnect bus; the difference value output by the second discrete subtractor becomes the horizontal coordinate value of the grid map that meets the bus bit width transmission requirements; the first input terminal of the second discrete multiplier is connected to the output terminal of the second discrete subtractor; the second input terminal of the second discrete multiplier is used to receive the inverse of the map resolution transmitted from the interconnect bus; the product value output by the second discrete multiplier is configured as the vertical coordinate value of the discrete point cloud to complete the coordinate system transformation and align the vertical coordinate value of the currently stored point cloud to the coordinate system of the grid map that meets the bus bit width transmission requirements; the second discrete multiplier is further used to output the product value obtained by multiplication to the memory module via the interconnect bus, so that the TSDF value calculation module can read the vertical coordinate value of the discrete point cloud.

[0020] For each point cloud, this technical solution reads the horizontal and vertical coordinates from the block storage space where the point cloud is stored, inputs them into the point cloud discretization submodule, and transforms the horizontal and vertical coordinates of the above point cloud from the point cloud coordinate system to the coordinate system of the pre-expanded grid map in the same offset manner along each adaptive coordinate axis direction through adders and subtractors. This includes swapping the coordinate axes of the point cloud to achieve the coordinate system transformation; the coordinates are then multiplied by the inverse of the grid map resolution to complete the discretization processing of the point cloud.

[0021] Furthermore, the point cloud processing module also includes a point cloud rotation submodule; the point cloud rotation submodule includes a first point cloud multiplier, a second point cloud multiplier, a third point cloud multiplier, a fourth point cloud multiplier, a point cloud adder and a point cloud subtractor, the first point cloud multiplier, the second point cloud multiplier, the third point cloud multiplier and the fourth point cloud multiplier are all multipliers, the point cloud adder is an adder, and the point cloud subtractor is a subtractor; the first input end of the first point cloud multiplier is connected to the output end of the first register; wherein, the point cloud discretization submodule is internally provided with a first register; the horizontal coordinate of the point cloud transmitted to the first input end of the first discrete adder by the interconnection bus is cached in the first register; the second input end of the first point cloud multiplier End, for receiving the cosine function value corresponding to the rotation angle achieved by the current rotation transformation, wherein the rotation angle achieved by the current rotation transformation is the sum of the angle search step and the rotation angle achieved by the previous rotation transformation; the angle search step is stored in the search window parameter register; the search window parameter register is a parameter register set inside the correlation scan matching hardware circuit, and is used to store the posture covered by the search window and the associated step search information; the first input end of the second point cloud multiplier is connected to the output end of the first register; wherein the point cloud discretization submodule is internally provided with a second register; the vertical coordinate of the point cloud transmitted to the first input end of the second discrete adder by the interconnection bus is cached to In the second register; the second input end of the second point cloud multiplier is used to receive the sine function value corresponding to the rotation angle achieved by the current rotation transformation; the first input end of the third point cloud multiplier is connected to the output end of the first register; the second input end of the third point cloud multiplier is used to receive the sine function value corresponding to the rotation angle achieved by the current rotation transformation; the first input end of the fourth point cloud multiplier is connected to the output end of the second register; the second input end of the fourth point cloud multiplier is used to receive the cosine function value corresponding to the rotation angle achieved by the current rotation transformation; the first input end of the point cloud subtractor is connected to the output end of the first point cloud multiplier, and the second input end of the point cloud subtractor is connected to the output end of the second point cloud multiplier. The first input terminal of the point cloud adder is connected to the output terminal of the third point cloud multiplier, and the second input terminal of the point cloud adder is connected to the output terminal of the fourth point cloud multiplier, wherein the sum value output by the point cloud adder is set to the ordinate value of the rotated point cloud obtained after the point cloud undergoes the current rotation transformation; the point cloud subtractor is also used to transmit the output difference value to the memory module through the interconnection bus and overwrite the ordinate value of the point cloud stored in the memory module, so as to complete the update of the ordinate result of the current rotation transformation to the ordinate value of the currently stored point cloud stored in the memory module; the first input terminal of the point cloud adder is connected to the output terminal of the third point cloud multiplier, and the second input terminal of the point cloud adder is connected to the output terminal of the fourth point cloud multiplier, wherein the sum value output by the point cloud adder is set to the ordinate value of the rotated point cloud obtained after the point cloud undergoes the current rotation transformation;The point cloud adder is further configured to transmit the output difference value to the memory module via the interconnect bus and overwrite the vertical coordinate of the point cloud stored in the memory module, thereby updating the vertical coordinate result of the current rotation transformation to the vertical coordinate of the currently stored point cloud stored in the memory module. The number of rotations is determined by the pre-configured angular search range and pre-configured angular search step of the search window. Each rotation transformation corresponds to a coordinate system transformation performed by the point cloud processing module and is included in a matching process of the correlation scan matching of the map.

[0022] This technical solution uses the rotation matrix as the basic operation architecture and only uses adders, subtractors, and multipliers to construct a point cloud rotation submodule for receiving and processing the trigonometric function values ​​corresponding to the step rotation angles pre-calculated by the software. This not only eliminates the need for a trigonometric function hardware calculation module, but also controls the point cloud rotation submodule to iteratively execute the rotation transformation based on the step rotation, obtaining a point cloud for updating the previously stored point cloud in the memory module. This technical solution, combined with the aforementioned point cloud discretization submodule, completes the traversal of each pose in the search window, thereby promoting the TSDF value calculation module to construct a weighted function for each pose in the search window.

[0023] Furthermore, the state machine control module is a finite state machine; the state machine control module is used to schedule the working states of the memory module, the point cloud processing module, and the TSDF value calculation module, so that each time the point cloud processing module performs the coordinate system transformation and the rotation transformation, the TSDF value calculation module performs the division operation once to obtain a TSDF value corresponding to a posture searched in the search window. On the basis of updating the currently stored point cloud using the result obtained by the rotation transformation, the point cloud processing module is controlled to continue to perform the coordinate system transformation and the TSDF value calculation module is controlled to perform the division operation again until the TSDF value calculation module counts that all postures in the search window have been searched and calculates the TSDF values ​​of all postures matching in sequence. The search step size is the number of grid searches that need to be traversed each time a posture is searched in the same search window; the search window is the neighborhood grid area centered on a point cloud in the pre-expanded grid map. In the present technical solution, the state machine control module completes the correlation scan matching corresponding to the posture in the search window in a hardware-based scheduling working status signal, including controlling the point cloud processing module to perform rotation and translation transformation on each point cloud in turn, under the premise that the TSDF value calculation module calculates the TSDF value of a posture in the search window, so as to read the truncated signed distance function value and weight value matching the index value from the memory module, realize the operation of the loop state, ensure the normal operation of the hardware-based correlation scan matching, and accelerate the convergence speed of the algorithm.

[0024] Furthermore, a bus interface module is provided outside the correlation scan matching hardware circuit, and the bus interface module includes a DMA controller module and a transmission bus; the DMA controller module is used to continuously transmit data stored in the physical storage space of discontinuous addresses in batches to reduce the number of times the CPU software interrupt is triggered; the transmission bus includes a first bus and a second bus, the first bus has signal transmission and reception connections with the memory module, the point cloud processing module, the TSDF value calculation module, the state machine control module, the interconnection bus and the DMA controller module respectively, the first bus is used to configure data transmission parameters for the DMA controller module, and the first bus is also used to configure the parameters stored in the map size register, the parameters stored in the map resolution register, the extended parameters required for the maximum detection radius reached by the currently stored point cloud, the extended parameters required for performing the coordinate system transformation, the extended parameters required for performing the rotation transformation, the extended parameters required to meet the transmission requirements of the bus bit width and the search step size, so as to realize the mapping communication of the memory of the correlation scan matching hardware circuit; the second bus is connected to the DMA controller module and is used to transmit the truncated signed distance function and its matching weight value pre-configured by the CPU to the memory module; wherein, the transmission bus complies with the AMBA protocol.

[0025] This technical solution provides a bus interface architecture module for the correlation scan matching hardware circuit and an external data source (controller). According to the data transmission performance, a first bus suitable for simple, low-throughput memory mapping communication (transmitting extended parameters, map feature parameters and associated basic control signals to the registers inside the correlation scan matching hardware circuit) and a second bus for high-speed data flow (transmitting truncated signed distance function values ​​and weight values ​​to the memory module) are designed respectively; thereby improving the real-time performance of the calculation work of the correlation scan matching hardware circuit.

[0026] A heterogeneous computing architecture, the heterogeneous computing architecture includes a processor unit, a bus interface and the correlation scan matching hardware circuit, the processor unit and the correlation scan matching hardware circuit are interconnected through the bus interface, the processor unit does not control the parallel acceleration operation performed by the correlation scan matching hardware circuit, and the processor unit only provides operation parameters, start signals, interrupt signals, reset signals and flag clear signals to the correlation scan matching hardware circuit through the bus interface; wherein, when the processor unit is a control circuit board independent of the correlation scan matching hardware circuit, the heterogeneous computing architecture is applied to a heterogeneous computing circuit system; wherein, when the processor unit, the bus interface and the correlation scan matching hardware circuit are integrated on the same chip, the heterogeneous computing architecture is applied to a heterogeneous chip. BRIEF DESCRIPTION OF THE DRAWINGS

[0027] Figure 1 It is a schematic diagram of a correlation scan matching hardware circuit based on a TSDF map according to an embodiment of the present invention.

[0028] Figure 2 It is a schematic diagram of a TSDF value calculation module disclosed in another embodiment of the present invention.

[0029] Figure 3 4 is a schematic diagram of a point cloud processing module disclosed in yet another embodiment of the present invention. DETAILED DESCRIPTION

[0030] The following is a further description of the specific embodiments of the present invention in conjunction with the accompanying drawings. It should be noted that the memory module described in the above embodiments is essentially a storage medium, which can be, but is not limited to, various storage media that can store program code, such as read-only memory (ROM), random access memory (RAM), etc.; the point cloud processing module, TSDF value calculation module, state machine control module, interconnection bus, and each computing unit in the point cloud processing module and each computing unit in the TSDF value calculation module in the correlation scan matching hardware circuit disclosed in the embodiment of the present invention can be, but is not limited to, a digital circuit module compiled by the designer using the hardware description language Verilog HDL, or a digital circuit module compiled by the designer using circuit drawing or compilation software. In addition, the functional units in various embodiments of the present invention can be integrated into a processing module, or each unit can exist physically separately, or two or more units can be integrated into a module.

[0031] The embodiment of the present invention discloses a correlation scan matching hardware circuit based on the TSDF map. The correlation scan matching hardware circuit is electrically connected to a laser radar and is used to calculate the TSDF value of the point cloud aligned to the grid map mapped onto the truncated signed distance field, and is used to evaluate the positioning accuracy of the currently collected laser point cloud in a posture within the search window of the grid map. TSDF has good scene reconstruction accuracy, especially in the field of mobile robot SLAM. The TSDF map is more suitable for optimization-based SLAM algorithms than the occupancy grid map. The laser radar can be installed on a mobile robot, and the laser probe of the laser radar continues to collect laser point cloud frame data during the rotation process, which is recorded as a point cloud; the scanning range of the laser radar can be based on the center of the robot body as the scanning center, or the center of the laser radar as the scanning center; in this embodiment, each time the laser radar rotates one circle, the point cloud collected in this circle is controlled to be sent to the correlation scan matching hardware circuit to calculate a TSDF value and match a posture within the search window. It should be noted that there is a specific coordinate transformation relationship between the world coordinate system and the two-dimensional grid map coordinate system. The position of the robot's body center in the two-dimensional grid map coordinate system is calculated based on this specific coordinate transformation relationship; then, based on the currently acquired point cloud data, the maximum range reached by the point cloud is counted.

[0032] The correlation scan matching hardware circuit includes a memory module, a point cloud processing module, a TSDF value calculation module, a state machine control module, and an interconnection bus. The memory module, the point cloud processing module, the TSDF value calculation module, and the state machine control module all transmit data via the interconnection bus. The interconnection bus, as the interconnection structure of the correlation scan matching hardware circuit, functions as data transmission and data sharing between the various circuit modules within the correlation scan matching hardware circuit. The point cloud processing module, the TSDF value calculation module, and the state machine control module are all connected to the memory space of the memory module via the interconnection structure. The interconnection bus communicates and transmits data with an external processor or an external bus via a bus interface provided by the correlation scan matching hardware circuit. The state machine control module, as a finite state machine, is configured as a control unit in the correlation scan matching hardware circuit to schedule data transmission on the interconnection bus. If the correlation scan matching hardware circuit is integrated into a chip, the interconnection bus is an on-chip interconnection bus responsible for on-chip data communication. However, the interconnection bus can communicate with an off-chip processor or other off-chip devices via a bus interface provided by the chip.

[0033] The point cloud processing module is used to read the currently stored point cloud from the memory module under the control of the state machine control module, specifically from Figure 1The existing point cloud is read in the box marked with Point Cloud, and then the read point cloud is controlled to perform coordinate system transformation, including the offset transformation of the horizontal and vertical coordinate values ​​and the scale adjustment of the map resolution, so as to realize the coordinate transformation first and then the discretization processing, and finally obtain the discretization result as the coordinate system transformation result defined in this embodiment. The result of the above-mentioned coordinate system transformation is set as a discrete point cloud, specifically falling into the coordinate system of the pre-expanded grid map, that is, the currently stored point cloud is aligned to the coordinate system of the pre-expanded grid map; then the result of the above-mentioned coordinate system transformation is stored in the memory module, specifically the discrete point cloud is stored in Figure 1 The box marked with Discrete Point Cloud is where the currently stored point cloud, i.e., the point cloud stored in the box marked with Point Cloud, is derived from the point cloud collected by the LiDAR and supports subsequent point cloud update and overwriting of transformations, so that the currently stored point cloud is not necessarily the original point cloud collected. It should be noted that, in the aforementioned grid map, the value in each grid represents the probability that the grid is occupied; the alignment of the point cloud with the pre-expanded grid map is understood to be the process of aligning the point cloud used to represent the obstacle scanned by the LiDAR with the obstacle grid in the pre-expanded grid map. Preferably, the point cloud processing unit calls the connected computing unit to perform rotation and translation to achieve point cloud and obstacle overlap.

[0034] The point cloud processing module is further configured to read the currently stored point cloud from the memory module under the control of the state machine control module, specifically from Figure 1 The existing point cloud is read in the box marked with Point Cloud, and then the read point cloud is controlled to perform a rotation transformation to achieve coverage of the angle search range of the search window by the preset angle search step of the point cloud. Preferably, this embodiment optimizes the rotation transformation of the currently read point cloud into a step rotation, ensuring that the angle of each rotation is obtained by rotating once by the preset angle search step on the basis of the angle obtained by the previous rotation, so that the rotation transformation does not start from the initial position every time; then the result of the aforementioned rotation transformation is updated to the currently stored point cloud stored in the memory module, that is, the point cloud existing in the box marked with Point Cloud is updated to cover, so that the updated point cloud (the currently stored point cloud) is configured to perform the aforementioned coordinate system transformation, and determine to complete a matching process of the correlation scan matching of the map; it should be emphasized that each matching process of the correlation scan matching of the map includes a coordinate system transformation, and the aforementioned transformation is performed based on the pre-expanded raster map.

[0035] The TSDF value calculation module is used to read the discrete point cloud from the memory module under the control of the state machine control module. Figure 1Read the discrete point cloud in the box marked as Discrete Point Cloud, and then calculate the index value of the discrete point cloud mapped to the storage space of the truncated signed distance function value according to the preset coordinate offset value and search step size, match the rotation and translation operation of the pre-expanded grid map, and set the index value to the read address of the truncated signed distance function value and the read address of the weight value stored in the memory module, specifically corresponding to Figure 1 Marked as Map oftsdf i The truncated signed distance function value tsdf in the box (x) i (x) read address, Figure 1 Marked as Map ofweight i The weight value weight in the box (x) i (x), where i represents the number of point clouds, i.e., the number of point clouds obtained by one scan of the laser radar; the truncated signed distance function value and the weight value both have a matching index value; the search step is the number of grid searches that need to be traversed each time a posture is searched within the same search window; it should be noted that the search window is a neighborhood grid area centered on a point cloud within the pre-expanded grid map, wherein there is a search window for each point cloud.

[0036] The TSDF value calculation module is also used to read the truncated signed distance function value tsdf from the memory module according to the currently set read address under the control of the state machine control module. i (x) and its matching weight value weight i (x), where the currently read truncated signed distance function value tsdf i The number of (x) is equal to the weight value currently read out i(x); then control the built-in divider of the TSDF value calculation module to equivalently complete the weighted average processing of the truncated signed distance function value mapped by the currently stored point cloud by performing a division operation, wherein, before performing the division operation, it also includes the accumulation and summation of the read data to obtain the dividend and divisor terms; then set the result of each operation output of the divider to the TSDF value. Compared with the prior art, this embodiment uses a hardware-based division operation to obtain the TSDF value, which is used to represent the positioning accuracy of a posture searched in the search window; in this embodiment, each time the point cloud processing module completes a matching process of the correlation scan matching of the map (including the coordinate system transformation and rotation transformation performed in succession), the built-in divider of the TSDF value calculation module performs a division operation to obtain the TSDF value corresponding to a new posture in the search window, as the result of the correlation scan matching hardware circuit performing an acceleration operation. Preferably, the truncated signed distance function value tsdf i (x) and the weight value weight to evaluate its uncertainty i (x) is proportional to.

[0037] The memory module is used to store the currently stored point cloud, the discrete point cloud obtained by transforming the currently stored point cloud through the point cloud processing module, the pre-set truncated signed distance function and its matching weight value; wherein each type of data is stored in different block storage spaces of the same memory module, specifically as Figure 1 As shown, the memory module is divided into the box marked with Point Cloud (which may belong to Block RAM), the box marked with Discrete Point Cloud (which may belong to Block RAM), and the box marked with Map of tsdf according to the storage content. i (x) box (may belong to Block RAM), the aforementioned mark is Map of weight i (x) box (may belong to BlockRAM).

[0038] Therefore, this embodiment calls the TSDF value calculation module to perform a division operation for each posture within the search window in the pre-expanded grid map to obtain the TSDF value corresponding to the posture, and the division operation is scheduled by the state machine control module based on the discrete point cloud provided by the point cloud processing module and the rotation transformation results for updating the discrete point cloud provided by the point cloud processing module, while ensuring that the associated operation results and transformation results are shared on the interconnection bus; compared with the existing technology, the hardware operation method of this technical solution reduces the amount of division operations in the hardware, does not increase the additional storage space of the hardware, and reduces the consumption of a large amount of programmable logic resources.

[0039] As an example, Figure 2 As shown, the TSDF value calculation module includes a selector, a frequency division counter, a pre-calculation unit, a divider, a first accumulator, a second accumulator and a third accumulator; the first accumulator is Figure 2 Marked as SUMtsdf i (x) box, used to control the truncated signed distance function value tsdf i (x) Perform cumulative summation, where the number of accumulated items is i, which is equal to the number of the currently stored point clouds. The second accumulator is Figure 2 Marked as SUM weight i (x) box, used to control the weight value weight i (x) performing cumulative summation, wherein the number of accumulated items is i, i is equal to the number of the currently stored point clouds. The third accumulator is Figure 2 Marked as SUM tsdf i (x)*weight i The box (x) is used to receive the truncated signed distance function value and the weight value belonging to the same i value, and then control the product of the truncated signed distance function value and the weight value belonging to the same i value to perform cumulative summation, wherein the number of accumulated items is i, and i is equal to the number of the currently stored point clouds. The frequency division counter is Figure 2 The box marked as Division Counter in the state machine control module; the TSDF value calculation module is used to send the truncated signed distance function value into the first accumulator under the control of the state machine control module, and send the weight value into the second accumulator at the same time, and send the product of the truncated signed distance function value and the weight value matched therewith into the third accumulator, and control each accumulator to perform the accumulation operation at the same time; wherein the input ends of the three accumulators are configured to receive the data transmitted in sequence by the interconnection bus; the output ends of the aforementioned three accumulators are respectively connected to the corresponding data input ends of the selector, the selection input end of the selector is connected to the counting output end of the frequency division counter, and the selector is provided with A data output terminal matched with each data input terminal is used to select the data received by the data input terminal of the output selector under the counting trigger of the frequency division counter to adapt to the enable timing of the divider. In this embodiment, the accumulated results output by the first accumulator, the accumulated results output by the second accumulator, and the accumulated results output by the third accumulator are simultaneously selected and output. Each data output terminal of the selector is respectively connected to the corresponding input terminal of the pre-calculation unit, and is used to transmit the accumulated sum value of the product of the truncated signed distance function value and the weight value, the accumulated sum value of the truncated signed distance function value, and the accumulated sum value of the weight value to the pre-calculation unit.

[0040] Specifically, the pre-calculation unit includes a numerator adder, a first numerator subtractor, a second numerator subtractor, a numerator shift register, a first numerator subtractor and a denominator shift register; the numerator adder is an adder, the first numerator subtractor and the second numerator subtractor are both subtractors, and the numerator shift register and the denominator shift register are both shift registers; wherein, the first input end of the second numerator subtractor is connected to the output end of the first accumulator, and the second input end of the second numerator subtractor is connected to the output end of the third accumulator; the second numerator subtractor is used to control the subtraction of the accumulated sum value output by the first accumulator and the accumulated sum value output by the third accumulator, and then output the difference between the two subtracted data.

[0041] The first numerator subtractor is provided with an input end for receiving the quantity value of the currently stored point cloud transmitted by the interconnection bus; the first numerator subtractor is provided with another input end connected to the output end of the second accumulator; the first numerator subtractor is used to control the subtraction of the accumulated sum value output by the second accumulator and the quantity value of the currently stored point cloud transmitted by the interconnection bus, and then output the difference between the two subtracted data, specifically the result of connecting the two input ends of the first numerator subtractor to the corresponding data output ends of the selector; the input end of the numerator shift register is connected to the output end of the first numerator subtractor, and the numerator shift register is used to shift the difference value output by the first numerator subtractor so that the binary digit width obtained by the shift processing of the difference value output by the first numerator subtractor is equal to the bit width of the accumulated sum value output by the first accumulator. Preferably, the number of shift bits performed by the numerator shift register is equal to the difference between the binary digit width equivalent to the truncated signed distance function value and the binary digit width equivalent to the weight value. The numerator adder is used to add the shift output result of the numerator shift register to the difference output by the second numerator subtractor, and then output the sum of the two added data to the corresponding input end of the divider, so that the current output sum is configured as the dividend.

[0042] A first numerator subtractor is used to control the subtraction of the accumulated sum value output by the second accumulator and the quantity value of the currently stored point cloud transmitted by the interconnection bus, and then output the difference between the two subtracted data, specifically the result of connecting the two input ends of the first numerator subtractor to the corresponding data output ends of the selector; the input end of the denominator shift register is connected to the output end of the first numerator subtractor, the denominator shift register is used to shift the difference value output by the first numerator subtractor, and then configure the data obtained by the shift processing as a divisor to output to the corresponding input end of the divider, so that the binary digit width obtained by the shift processing of the difference value output by the first numerator subtractor is one bit lower than the binary digit width of the accumulated sum value output by the first accumulator, and the divisor output by the first numerator subtractor is configured as the frequency division coefficient of the divider.

[0043] The two input ends of the divider are respectively connected to the output end of the numerator adder and the output end of the denominator shift register. The divider is used to enable the divider after being enabled, specifically when the divisor and dividend calculated by the hardware unit are assigned to the input port of the divider. There are dozens of pipelines inside the divider. Under the counting trigger of the frequency divider counter, after several clock cycles, the divider output is enabled, and then the dividend output by the numerator adder and the divisor output by the denominator shift register are controlled to perform a division operation, and then the calculated quotient value is sent back to the interconnection bus to determine that the quotient value becomes the TSDF value corresponding to a posture within the search window, and also to determine that the divider completes a weighted average processing of the truncated signed distance function value mapped by the currently stored point cloud or the weight increment update in the software algorithm by performing a division operation.

[0044] Compared with the prior art, the TSDF value calculation module treats the weighted average processing of the truncated signed distance function value as equivalent to an integral operation with the truncated distance as the integral variable and the number of currently stored point clouds as the integral upper limit. Therefore, the present technical solution first merges the truncated signed distance function value, the weight, and the product of the truncated signed distance function value and the weight into similar terms (accumulated sum terms) in hardware, then performs parallel accumulation processing on these similar terms, selects the accumulated terms of the weight as the frequency division parameter of the divider, and then performs a division operation based on the shift processing of both the dividend and the divisor. Under the counting trigger of the frequency division counter, all the truncated signed distance function values ​​and weight values ​​corresponding to the current iteration state of the correlation scan matching algorithm of the point cloud processing module can be pushed into the TSDF value calculation module to perform a division operation. This saves the computing resources of the divider.

[0045] As an embodiment, combining Figure 1 and Figure 2 It can be seen that the TSDF value calculation module includes a grid index submodule, and the grid index submodule includes a first index subtractor, a second index subtractor, a first index adder, a second index adder, a third index adder and an index multiplier; wherein the first index subtractor and the second index subtractor are both subtractors. Figure 2 The grid index submodule is a circle marked with "-"; the first index adder, the second index adder and the third index adder are all adders. Figure 2 The grid index submodule is a circle marked with "+"; the index multiplier belongs to the multiplier. Figure 2 The Grid Index submodule of the Grid Index submodule is a circle marked with an "×".

[0046] The first input end of the first index subtractor is used to receive the horizontal coordinates of the discrete point cloud transmitted from the interconnection bus. Preferably, the horizontal coordinates of the discrete point cloud transmitted from the interconnection bus support being stored in a register inside the grid index submodule. The first input end of the first index subtractor receives the horizontal coordinates of the discrete point cloud transmitted from the interconnection bus through the connected register. Figure 2 In the embodiment shown, the first input terminal of the first index subtractor (at Figure 2 The first subtractor arranged from top to bottom in the grid index submodule) and register DP x (marked with DP x ) are connected, wherein register DP x It is used to cache the horizontal coordinates of the discrete point cloud transmitted from the interconnection bus, and then transmit the horizontal coordinates of the currently cached discrete point cloud to the first input end of the first index subtractor, so that the first input end of the first index subtractor completes receiving the horizontal coordinates of the discrete point cloud transmitted from the interconnection bus; wherein, the discrete point cloud transmitted to the first index subtractor by the interconnection bus is derived from the memory module. The second input end of the first index subtractor is used to receive the horizontal axis coordinate offset value transmitted from the interconnection bus. Preferably, the horizontal axis coordinate offset value transmitted from the interconnection bus supports being stored in a register inside the grid index submodule, and the second input end of the first index subtractor receives the horizontal axis coordinate offset value transmitted from the interconnection bus through the connected register. Figure 2 In the embodiment shown, the second input terminal of the first index subtractor is connected to the register Offset x (Marked with Offset x The box) is connected, where the register Offset x The device is configured to cache the horizontal axis coordinate offset value transmitted from the interconnect bus, and then transmit the currently cached horizontal axis coordinate offset value to the second input terminal of the first index subtractor, so that the second input terminal of the first index subtractor can complete receiving the horizontal axis coordinate offset value transmitted from the interconnect bus. The preset coordinate offset value includes the horizontal axis coordinate offset value, and the horizontal axis coordinate offset value transmitted from the interconnect bus is derived from the preset coordinate offset value stored in the map offset value register; the map offset value register is a parameter register set within the correlation scan matching hardware circuit, and is used to store the coordinate offset value associated with the pre-expanded grid map. Figure 1 The parameter register group shown includes a map offset value register. A first index subtractor is used to control the subtraction of the horizontal coordinate of the discrete point cloud received at the first input terminal and the horizontal axis coordinate offset value received at the second input terminal, and then output the difference.

[0047] The first input end of the first index adder is used to receive the difference output by the first index subtractor. In this embodiment, the first input end of the first index adder is connected to the output end of the first index subtractor to construct a combinational logic circuit; the second input end of the first index adder is used to receive the horizontal axis coordinate search step transmitted from the interconnection bus; preferably, the horizontal axis coordinate search step transmitted from the interconnection bus supports storage in a register inside the grid index submodule, and the second input end of the first index adder receives the horizontal axis coordinate search step transmitted from the interconnection bus through the connected register. Figure 2 In the embodiment shown, the second input terminal of the first index adder is connected to the register Step x (Marked with Step x The block) is connected, where the register Step x The first index adder is configured to cache the horizontal axis coordinate search step length transmitted from the interconnect bus, and then transmit the currently cached horizontal axis coordinate search step length to the second input terminal of the first index adder. The search step length includes the horizontal axis coordinate search step length, and the horizontal axis coordinate search step length transmitted from the interconnect bus is derived from the search step length stored in the search window parameter register, and the search step length includes the horizontal axis coordinate search step length; the search window parameter register is a parameter register set within the correlation scan matching hardware circuit, and is used to store the pose covered by the search window and associated search information; Figure 1 The parameter register group shown includes a search window parameter register. A first index adder is configured to control the addition of the difference between the horizontal coordinate search step size and the output of the first index subtractor, output the sum of the additions, and determine the sum as the horizontal axis index value in the storage space for mapping the discrete point cloud to the truncated signed distance function value.

[0048] The first input end of the second index subtractor is used to receive the vertical coordinates of the discrete point cloud transmitted from the interconnection bus. Preferably, the vertical coordinates of the discrete point cloud transmitted from the interconnection bus support being stored in a register inside the grid index submodule. The first input end of the second index subtractor receives the vertical coordinates of the discrete point cloud transmitted from the interconnection bus through the connected register. Figure 2 In the embodiment shown, the first input terminal of the second index subtractor (at Figure 2 The second subtractor arranged from top to bottom in the grid index submodule) and register DP y (marked with DP y The output terminal of the block) is connected, wherein the register DP yThe method is used to cache the vertical coordinates of the discrete point cloud transmitted by the interconnection bus, and then transmit the vertical coordinates of the currently cached discrete point cloud to the first input end of the second index subtractor, so that the first input end of the second index subtractor completes receiving the vertical coordinates of the discrete point cloud transmitted by the interconnection bus; wherein, the discrete point cloud transmitted to the second index subtractor by the interconnection bus comes from the memory module.

[0049] The second input end of the second index subtractor is used to receive the vertical axis coordinate offset value transmitted from the interconnection bus. Preferably, the vertical axis coordinate offset value transmitted from the interconnection bus supports being stored in a register inside the grid index submodule. The second input end of the second index subtractor receives the vertical axis coordinate offset value transmitted from the interconnection bus through the connected register. Figure 2 In the embodiment shown, the second input terminal of the second index subtractor is connected to the register Offset y (Marked with Offset y The box) is connected, where the register Offset y The second index subtractor is configured to cache the vertical coordinate offset value transmitted from the interconnect bus and then transmit the currently cached vertical coordinate offset value to the second input of the second index subtractor; wherein the preset coordinate offset value also includes the vertical coordinate offset value, and the vertical coordinate offset value transmitted from the interconnect bus is also derived from the preset coordinate offset value stored in the map offset value register. The second index subtractor is configured to control the subtraction of the vertical coordinate of the discrete point cloud received from the first input and the vertical coordinate offset value received from the second input, and then output the difference.

[0050] The first input end of the second index adder is used to receive the difference output by the second index subtractor. In this embodiment, the first input end of the second index adder is connected to the output end of the second index subtractor to construct a combinational logic circuit; the second input end of the second index adder is used to receive the vertical axis coordinate search step transmitted from the interconnection bus. Preferably, the vertical axis coordinate search step transmitted from the interconnection bus supports storage in a register inside the grid index submodule, and the second input end of the second index adder receives the vertical axis coordinate search step transmitted from the interconnection bus through the connected register. Figure 2 In the embodiment shown, the second input terminal of the second index adder is connected to the register Step y (Marked with Step y The block) is connected, where the register Step yThe second index adder is configured to cache the vertical axis coordinate search step transmitted from the interconnect bus and then transmit the currently cached vertical axis coordinate search step to the second input of the second index adder. The search step also includes the vertical axis coordinate search step, and the vertical axis coordinate search step transmitted from the interconnect bus also originates from the search step stored in the search window parameter register; the search step also includes the vertical axis coordinate search step. The second index adder is configured to control the addition of the vertical axis coordinate search step and the difference between the output of the second index subtractor, output the sum of the addition, and determine the sum as the vertical axis direction index value within the storage space where the discrete point cloud is mapped to the truncated signed distance function value.

[0051] The above-mentioned embodiment uses two identical addition and subtraction operation combination structures to respectively calculate the horizontal axis direction index value in the storage space mapped to the truncated signed distance function value, and the vertical axis direction index value in the storage space mapped to the truncated signed distance function value, thereby realizing the parallel conversion of the horizontal coordinate and the vertical coordinate of the discrete point cloud into discrete index information in the storage space of the truncated signed distance function value.

[0052] As a preferred example, combined with Figure 1 and Figure 2 It can be seen that the grid index submodule also includes a third index adder and an index multiplier; the third index adder belongs to the adder, corresponding to Figure 2 The first adder arranged from left to right in the grid index submodule of Figure 2 The only circle marked with an “×” in the Grid Index submodule of .

[0053] The first input of the index multiplier is used to receive the sum output by the second index adder. In this embodiment, the first input of the index multiplier is connected to the output of the second index adder to construct a combinational logic circuit. The second input of the index multiplier is used to receive the number of row grids transmitted from the interconnection bus. Preferably, the number of row grids transmitted from the interconnection bus supports storage in a register inside the grid index submodule. The second input of the index multiplier receives the number of row grids transmitted from the interconnection bus through the connected register. Figure 2 In the embodiment shown, the second input terminal of the index multiplier is connected to the register NUM x (marked with NUM x The block) is connected, where register NUM xfor caching the number of row grids transmitted from the interconnection bus, and then transmitting the currently cached number of row grids to the second input terminal of the index multiplier, so that the second input terminal of the index multiplier completes receiving the number of row grids transmitted from the interconnection bus; wherein the number of row grids is the number of grids that can be occupied by each row of the storage space of the truncated signed distance function value, is stored in a map size register, and is transmitted to the interconnection bus by the map size register; Figure 1 The parameter register set shown includes the map size register. The map size register is a parameter register set within the correlation scan matching hardware circuit and is used to store the size range of the pre-expanded grid map and associated expansion information. An index multiplier is used to control the multiplication of the number of row grids by the sum output by the second index adder, and then output the product.

[0054] The first input end of the third index adder is used to receive the horizontal axis index value output by the first index adder, and the second input end of the third index adder is used to receive the product output by the index multiplier. In this embodiment, the first input end of the third index adder is connected to the output end of the first index adder, and the second input end of the third index adder is connected to the output end of the index multiplier to construct a combinational logic circuit; the third index adder is used to control the product of the number of row grids and the sum value output by the second index adder and add the horizontal axis index value, and then set the sum value obtained by the addition as the index value in the storage space where the discrete point cloud is mapped to the truncated signed distance function value, and send it to the interconnection bus, so as to read the truncated signed distance function value and the weight value matching the index value from the memory module. Therefore, in this preferred embodiment, an index multiplier controls the multiplication of the number of row grids and the vertical axis index value, and a third index adder controls the addition of the horizontal axis index value and the product output by the index multiplier. The index value of the discrete point cloud mapped to the storage space of the truncated signed distance function value is calculated in a row scanning query mode, and serves as the index value or address of the linear storage space, so that the interconnect bus can read the truncated signed distance function value and weight value matching the index value from the memory module. This is suitable for data parallel algorithms and promotes real-time processing of truncated signed distance function values ​​and their weight values ​​at high frame rates.

[0055] As another preferred example, the grid index submodule further includes a third index adder and an index multiplier, the third index adder belongs to the adder, and the index multiplier belongs to the multiplier; the circuit connection relationship of this preferred example is not shown in the drawings of the specification. The first input end of the index multiplier is used to receive the sum value output by the first index adder; the second input end of the index multiplier is used to receive the number of column grids transmitted from the interconnection bus; wherein, the number of column grids is the number of grids that each column of the storage space of the truncated signed distance function value can occupy, which is stored in the map size register and transmitted to the interconnection bus by the map size register; the map size register is a parameter register set inside the correlation scan matching hardware circuit, which is used to store the size range of the pre-expanded grid map and the associated expansion information; the index multiplier is used to control the number of column grids and the first index adder The sum value output by the multiplier is multiplied, and the product is output; the first input end of the third index adder is used to receive the longitudinal axis index value output by the second index adder, and the second input end of the third index adder is used to receive the product output by the index multiplier; the third index adder is used to control the product of the number of column grids and the sum value output by the first index adder and add the longitudinal axis index value, and then set the sum value obtained by the addition as the index value in the storage space of the discrete point cloud mapped to the truncated signed distance function value, and send it to the interconnection bus, so as to read the truncated signed distance function value and the weight value matching the index value from the memory module. With respect to the above-mentioned preferred example of obtaining index values ​​by row scanning, the index multiplier controls the multiplication of the number of column grids and the horizontal axis index value, and then the third index adder controls the addition of the vertical axis index value and the product output by the index multiplier, and calculates the index value in the storage space of the truncated signed distance function value mapped to the discrete point cloud in a column scanning query manner as the index value or address of the linear storage space, so that the interconnection bus can read the truncated signed distance function value and the weight value matching the index value from the memory module.

[0056] It should be noted that the pre-expanded grid map is based on the map delineated by the original point cloud, and is obtained by expanding and transforming the grid map in accordance with the expansion parameters required for the maximum detection radius reached by the currently stored point cloud, the expansion parameters required for executing the coordinate system transformation, the expansion parameters required for executing the rotation transformation, and the expansion parameters required for aligning the bus bit width; the aforementioned expansion parameters are also stored in the parameter register group and are used as parameters for the hardware to execute the rotation and translation of the point cloud. The aforementioned expansion parameters are all related to the maximum range reached by the point cloud. The specific expansion parameters and the calculation method of the expansion transformation of the grid map are prior art and will not be repeated here. In which, the original point cloud is all laser point clouds obtained by the current scan of the laser radar; the map defined by the original point cloud is a grid map defined with the laser radar as the center and the maximum detection diameter reached in the original point cloud as the map side length, or the grid map constructed by the system by default is clipped with the robot as the center and the maximum range reached by the point cloud as the radius; the alignment bus width means that the memory of the grid covered by the side length of the pre-expanded grid map is equal to the bit width of the bus transmitting the grid map; wherein, the preset coordinate offset value is determined by the sum of the coordinate translation parameters required for the maximum detection radius reached by the currently stored point cloud, the coordinate translation parameters required to perform the coordinate system transformation, the coordinate translation parameters required to perform the rotation transformation, and the coordinate translation parameters required for the alignment bus width. Preferably, the preset coordinate offset value is equal to the sum of the coordinate translation parameters required for the maximum detection radius reached by the currently stored point cloud, the coordinate translation parameters required to perform the coordinate system transformation, the coordinate translation parameters required to perform the rotation transformation, and the coordinate translation parameters required for the alignment bus width. The pre-expanded grid map is based on the maximum coverage radius of the point cloud, and is expanded in sequence according to the coordinate translation parameters, coordinate rotation parameters, and memory parameters occupied by the map side length. This can not only constrain the actual usage size of the map, but also adapt to the bus bit width used during map transmission, ensure data alignment, and reduce the storage space occupied by the TSDF map.

[0057] As an embodiment, combining Figure 1 and Figure 3 It can be seen that the point cloud processing module includes a point cloud discretization submodule; the point cloud discretization submodule includes a first discrete adder, a first discrete subtractor, a first discrete multiplier, a second discrete adder, a second discrete subtractor and a second discrete multiplier; the first discrete adder and the second discrete adder are both adders. Figure 3 The point cloud discretization submodule is a circle marked with "+"; the first discrete subtractor and the second discrete subtractor are both subtractors, which are shown in Figure 3 The point cloud discretization submodule is a circle marked with “-”; the first discrete multiplier and the second discrete multiplier are both multipliers, which are expressed as Figure 3 The circle in the point cloud discretization submodule is marked with “×”.

[0058] The first input end of the first discrete adder is used to receive the horizontal coordinates of the point cloud transmitted from the interconnection bus. Preferably, the horizontal coordinates of the point cloud transmitted from the interconnection bus support being stored in a register inside the point cloud discretization submodule. The first input end of the first discrete adder receives the horizontal coordinates of the point cloud transmitted from the interconnection bus through the connected register. Figure 3 In the embodiment shown, the first input terminal of the first discrete adder (at Figure 3 In the point cloud discretization submodule, the first adder arranged from top to bottom) and register P x (Marked with P x The block) is connected, wherein the register P x Used to cache the horizontal coordinates of the point cloud transmitted by the interconnection bus, and then transmit the horizontal coordinates of the currently cached point cloud to the first input end of the first discrete adder, so that the first input end of the first discrete adder completes receiving the horizontal coordinates of the point cloud transmitted by the interconnection bus; wherein, the point cloud transmitted to the first discrete adder by the interconnection bus is derived from the currently stored point cloud stored in the memory module, specifically from the block storage space corresponding to the aforementioned box marked with Point Cloud; the horizontal coordinate of the point cloud belongs to the horizontal coordinate value of the laser point cloud in the laser coordinate system.

[0059] The second input end of the first discrete adder is used to receive the horizontal coordinate of the robot position transmitted from the interconnection bus. Preferably, the horizontal coordinate of the robot position transmitted from the interconnection bus supports storage in a register inside the point cloud discretization submodule. The second input end of the first discrete adder receives the horizontal coordinate of the robot position transmitted from the interconnection bus through the connected register. Figure 3 In the embodiment shown, the second input terminal of the first discrete adder is connected to the register Pose x (marked with Pose x The box) is connected, where the register Pose x The first discrete adder is configured to cache the horizontal coordinate of the robot position transmitted from the interconnect bus and then transmit the currently cached horizontal coordinate of the robot position to the second input of the first discrete adder. The horizontal coordinate of the robot position is the horizontal coordinate value of the robot in the world coordinate system, pre-calculated (by software or CPU unit); the laser radar is mounted on the robot and serves as a sensor device for robot positioning. The first discrete adder is configured to control the addition of the horizontal coordinate of the point cloud and the horizontal coordinate of the robot position, and then output the summed value, so that the summed value becomes the horizontal coordinate value of the point cloud converted to the world coordinate system.

[0060] The first input end of the first discrete subtractor is used to receive the sum value output by the first discrete adder. In this embodiment, the first input end of the first discrete subtractor is connected to the output end of the first discrete adder to construct a combinational logic circuit; the second input end of the first discrete subtractor is used to receive the maximum horizontal coordinate value of the map transmitted from the interconnection bus. Preferably, the maximum horizontal coordinate value of the map transmitted from the interconnection bus supports storage in a register inside the point cloud discrete submodule. The second input end of the first discrete subtractor receives the maximum horizontal coordinate value of the map transmitted from the interconnection bus through the connected register. Figure 3 In the embodiment shown, the second input terminal of the first discrete subtractor is connected to the register MAX x (Marked with MAX x The block) is connected, where register MAX x The device is configured to cache the maximum horizontal coordinate value of the map transmitted from the interconnection bus and then transmit the currently cached maximum horizontal coordinate value of the map to the second input terminal of the first discrete subtractor. The maximum horizontal coordinate value of the map is derived from a map size register and transmitted to the interconnection bus from the map size register; wherein the map size register is a parameter register set within the correlation scan matching hardware circuit and is configured to store the size range of the grid map that meets the transmission requirements of the bus bit width; Figure 1 The parameter register group shown includes a map size register. A first discrete subtractor is configured to control the subtraction of the maximum horizontal coordinate value of the map from the sum output by the first discrete adder, output the difference obtained by the subtraction, and set the difference as the vertical coordinate value for transforming the point cloud to the grid map that meets the bus bit width transmission requirements.

[0061] The first input end of the first discrete multiplier is used to receive the difference output by the first discrete subtractor. In this embodiment, the first input end of the first discrete multiplier is connected to the output end of the first discrete subtractor to construct a combinational logic circuit; the second input end of the first discrete multiplier is used to receive the inverse of the map resolution transmitted from the interconnection bus. Preferably, the inverse of the map resolution transmitted from the interconnection bus supports storage in a register inside the point cloud discrete submodule. The second input end of the first discrete multiplier receives the inverse of the map resolution transmitted from the interconnection bus through the connected register. Figure 3In the illustrated embodiment, the second input of the first discrete multiplier is connected to register 1 / R (a block labeled 1 / R). Register 1 / R is used to cache the inverse of the map resolution transmitted from the interconnection bus and then transmit the currently cached inverse of the map resolution to the second input of the first discrete multiplier, allowing the second input of the first discrete multiplier to complete receiving the inverse of the map resolution transmitted from the interconnection bus. The inverse of the map resolution is derived from a map resolution register and transmitted to the interconnection bus by the map resolution register. The map resolution register is a parameter register set within the correlation scan matching hardware circuit and is used to store resolution information of the raster map that meets the transmission requirements of the bus bit width. Figure 1 The parameter register group shown includes a map resolution register. A first discrete multiplier is used to control the multiplication of the inverse of the map resolution and the difference output by the first discrete subtractor, and then output the product value obtained by the multiplication. This product value is configured as the horizontal coordinate value of the discrete point cloud to complete the coordinate system transformation and align the horizontal coordinate value of the currently stored point cloud to the coordinate system of the raster map that meets the bus bit width transmission requirements. The first discrete multiplier is also used to output the product value obtained by the multiplication to the memory module via the interconnect bus, and store it in the block storage space corresponding to the box marked with Discrete Point Cloud, so that the TSDF value calculation module can read the horizontal coordinate value of the discrete point cloud.

[0062] The first input end of the second discrete adder is used to receive the vertical coordinate of the point cloud transmitted from the interconnection bus. Preferably, the vertical coordinate of the point cloud transmitted from the interconnection bus supports being stored in a register inside the point cloud discretization submodule. The first input end of the second discrete adder receives the vertical coordinate of the point cloud transmitted from the interconnection bus through the connected register. Figure 3 In the embodiment shown, the first input terminal of the second discrete adder (at Figure 3 In the point cloud discretization submodule, the second adder arranged from top to bottom) and register P y (Marked with P y The block) is connected, wherein the register P y Used to cache the vertical coordinates of the point cloud transmitted by the interconnection bus, and then transmit the vertical coordinates of the currently cached point cloud to the first input end of the second discrete adder, so that the first input end of the second discrete adder completes receiving the vertical coordinates of the point cloud transmitted by the interconnection bus; wherein, the point cloud transmitted to the second discrete adder by the interconnection bus is derived from the currently stored point cloud stored in the memory module, specifically from the block storage space corresponding to the aforementioned box marked with Point Cloud; the vertical coordinate of the point cloud belongs to the vertical coordinate value of the laser point cloud in the laser coordinate system.

[0063] The second input end of the second discrete adder is used to receive the vertical coordinate of the robot position transmitted from the interconnection bus. Preferably, the vertical coordinate of the robot position transmitted from the interconnection bus supports storage in a register inside the point cloud discretization submodule. The second input end of the second discrete adder receives the vertical coordinate of the robot position transmitted from the interconnection bus through the connected register. Figure 3 In the embodiment shown, the second input terminal of the second discrete adder is connected to the register Pose y (marked with Pose y The box) is connected, where the register Pose y The second discrete adder is configured to cache the vertical coordinate of the robot position transmitted from the interconnect bus and then transmit the currently cached vertical coordinate of the robot position to the second input of the second discrete adder. The vertical coordinate of the robot position is a pre-calculated vertical coordinate value of the robot in the world coordinate system. The second discrete adder is configured to control the addition of the vertical coordinate of the point cloud and the vertical coordinate of the robot position, and then output the sum, so that the sum becomes the vertical coordinate value of the point cloud converted into the world coordinate system.

[0064] The first input end of the second discrete subtractor is used to receive the sum value output by the second discrete adder. In this embodiment, the first input end of the second discrete subtractor is connected to the output end of the second discrete adder to construct a combinational logic circuit; the second input end of the second discrete subtractor is used to receive the maximum vertical coordinate value of the map transmitted from the interconnection bus. Preferably, the maximum vertical coordinate value of the map transmitted from the interconnection bus supports storage in a register inside the point cloud discrete submodule. The second input end of the second discrete subtractor receives the maximum vertical coordinate value of the map transmitted from the interconnection bus through the connected register. Figure 3 In the embodiment shown, the second input terminal of the second discrete subtractor is connected to the register MAX y (Marked with MAX y The block) is connected, where register MAX y The second discrete subtractor is configured to cache the maximum vertical coordinate value of the map transmitted from the interconnect bus and then transmit the currently cached maximum vertical coordinate value of the map to the second input of the second discrete subtractor. The maximum vertical coordinate value of the map is derived from the map size register and transmitted to the interconnect bus by the map size register. The second discrete subtractor is configured to control the subtraction of the maximum vertical coordinate value of the map from the sum output by the second discrete adder, output the difference obtained by the subtraction, and set the difference as the horizontal coordinate value for transforming the point cloud to the grid map that meets the bus bit width transmission requirements.

[0065] The first input end of the second discrete multiplier is used to receive the difference output by the second discrete subtractor. In this embodiment, the first input end of the second discrete multiplier is connected to the output end of the second discrete subtractor to construct a combinational logic circuit; the second input end of the second discrete multiplier is used to receive the inverse of the map resolution transmitted from the interconnection bus. Preferably, the inverse of the map resolution transmitted from the interconnection bus supports storage in a register inside the point cloud discrete submodule. The second input end of the second discrete multiplier receives the inverse of the map resolution transmitted from the interconnection bus through the connected register. Figure 3 In the illustrated embodiment, the second input of the second discrete multiplier is connected to the register 1 / R (the box marked with 1 / R); the second discrete multiplier is used to control the multiplication of the inverse of the map resolution and the difference output by the second discrete subtractor, and then output the product value obtained by the multiplication, and configure the product value as the vertical coordinate value of the discrete point cloud to complete the coordinate system transformation and realize the alignment of the vertical coordinate value of the currently stored point cloud to the coordinate system of the raster map that meets the transmission requirements of the bus bit width; the second discrete multiplier is also used to output the product value obtained by the multiplication to the memory module through the interconnection bus, and store it in the block storage space corresponding to the box marked with Discrete Point Cloud, so that the TSDF value calculation module can read the vertical coordinate value of the discrete point cloud.

[0066] Therefore, an embodiment of the point cloud discretization submodule reads the horizontal coordinate and the vertical coordinate from the block storage space storing the point cloud for each point cloud, inputs the horizontal and vertical coordinates of the point cloud into the point cloud discretization submodule, and transforms the horizontal and vertical coordinates of the point cloud from the point cloud coordinate system to the coordinate system of the pre-expanded grid map in the same offset manner along the directions of the adaptive coordinate axes through adders and subtractors, including swapping the coordinate axes of the point cloud to achieve the coordinate system transformation; and then multiplies them with the inverse of the grid map resolution to complete the discretization processing of the point cloud.

[0067] Based on the above embodiments, Figure 1 and Figure 3 It can be seen that the point cloud processing module also includes a point cloud rotation submodule; the point cloud rotation submodule includes a first point cloud multiplier, a second point cloud multiplier, a third point cloud multiplier, a fourth point cloud multiplier, a point cloud adder and a point cloud subtractor; the first point cloud multiplier, the second point cloud multiplier, the third point cloud multiplier and the fourth point cloud multiplier are all multipliers, which are expressed as Figure 3 The point cloud rotation submodule is a circle marked with “×”; the point cloud adder belongs to the adder, which is shown as Figure 3 The point cloud rotation submodule is a circle marked with "+"; the point cloud subtractor belongs to the subtractor and is expressed as Figure 3 The circle marked with “-” is in the Point Cloud Rotation submodule of .

[0068] The first input end of the first point cloud multiplier is used to receive the same data as the horizontal coordinate of the point cloud received by the first input end of the first discrete adder, wherein the point cloud discretization submodule is provided with a first register, and the first input end of the first point cloud multiplier is connected to an output end of the first register. The horizontal coordinate of the point cloud transmitted by the interconnection bus is supported to be stored in the first register inside the point cloud discretization submodule. Figure 3 In the embodiment shown, the first input terminal of the first point cloud multiplier (at Figure 3 In the point cloud rotation submodule, the first multiplier arranged from top to bottom) and register P x (Marked with P x The output terminal of the block) is connected to the register P x (Marked with P x The other output terminal of the block) is connected to the first input terminal of the first discrete adder, wherein the register P x is the first register. Therefore, the horizontal coordinate of the point cloud transmitted by the interconnect bus to the first input terminal of the first discrete adder is first buffered in the first register and then output by the first register to the first input terminal of the first discrete adder and the first input terminal of the first point cloud multiplier.

[0069] The second input end of the first point cloud multiplier is used to receive the cosine function value corresponding to the rotation angle achieved by the current rotation transformation, wherein the rotation angle achieved by the current rotation transformation is the sum of the angle search step and the rotation angle achieved by the previous rotation transformation, and is the rotation angle and its corresponding cosine function value obtained from the step rotation operation performed at the CPU or software level, thereby avoiding increasing the complexity of the hardware unit due to trigonometric function operations and preventing hardware transmission delays. Preferably, the cosine function value transmitted by the interconnection bus supports storage in the register inside the point cloud rotation submodule, and the second input end of the first point cloud multiplier receives the cosine function value transmitted by the interconnection bus through the connected register. Figure 3 In the illustrated embodiment, the second input of the first point cloud multiplier is connected to the output of register COS (box labeled COS), wherein register COS is used to cache the cosine function value transmitted by the interconnect bus and then transmit the currently cached cosine function value to the second input of the first point cloud multiplier. The first point cloud multiplier is used to control the multiplication of the horizontal coordinate of the point cloud received at the first input of the first point cloud multiplier with the cosine function value corresponding to the rotation angle achieved by the current rotation transformation received at the second input of the first point cloud multiplier, and then output the product value.

[0070] The first input terminal of the second point cloud multiplier is used to receive the same data as the vertical coordinate of the point cloud received by the first input terminal of the second discrete adder; wherein, the point cloud discretization submodule is provided with a second register, and the first input terminal of the second point cloud multiplier is connected to an output terminal of the second register. The vertical coordinate of the point cloud transmitted by the interconnection bus is supported to be stored in the second register inside the point cloud discretization submodule. Figure 3 In the embodiment shown, the first input terminal of the second point cloud multiplier (at Figure 3 In the point cloud rotation submodule, the second multiplier arranged from top to bottom) and register P y (Marked with P y The output terminal of the block) is connected to the register P y (Marked with P y The other output terminal of the block) is connected to the first input terminal of the second discrete adder, wherein the register P y Therefore, the vertical coordinate of the point cloud transmitted by the interconnection bus to the first input terminal of the second discrete adder is first cached in the second register and then output by the second register to the first input terminal of the second point cloud multiplier and the first input terminal of the second discrete adder.

[0071] The second input end of the second point cloud multiplier is used to receive the sine function value corresponding to the rotation angle achieved by the current rotation transformation; wherein, the rotation angle achieved by the current rotation transformation is the sum of the angle search step and the rotation angle achieved by the previous rotation transformation, and is derived from the rotation angle and its corresponding sine function value obtained by the step rotation operation performed at the CPU or software level, thereby avoiding increasing the complexity of the hardware unit due to trigonometric function operations and preventing hardware transmission delays. Preferably, the sine function value transmitted by the interconnection bus supports storage in the register inside the point cloud rotation submodule, and the second input end of the second point cloud multiplier receives the sine function value transmitted by the interconnection bus through the connected register. Figure 3 In the illustrated embodiment, the second input of the second point cloud multiplier is connected to the output of register SIN (the box labeled SIN). Register SIN is configured to cache the sine function value transmitted by the interconnect bus and then transmit the cached sine function value to the second input of the second point cloud multiplier. The second point cloud multiplier is configured to control the multiplication of the vertical coordinate of the point cloud received at the first input of the second point cloud multiplier with the sine function value corresponding to the rotation angle achieved by the current rotation transformation received at the second input of the second point cloud multiplier, and then output the resulting product.

[0072] The first input terminal of the third point cloud multiplier is used to receive the horizontal coordinate of the point cloud output by the first register, which is the horizontal coordinate of the point cloud currently received by the first input terminal of the first discrete adder. Figure 3 In the embodiment shown, the first input terminal of the third point cloud multiplier (at Figure 3 In the point cloud rotation submodule, the third multiplier arranged from top to bottom) and register P x (Marked with P x The output terminal of the block) is connected, wherein the register P x is the first register. Therefore, the horizontal coordinate of the point cloud transmitted by the interconnection bus to the first input terminal of the first discrete adder is first buffered in the first register and then output by the first register to the first input terminal of the third point cloud multiplier and the first input terminal of the first point cloud multiplier.

[0073] The second input end of the third point cloud multiplier is used to receive the sine function value corresponding to the rotation angle achieved by the current rotation transformation; preferably, the sine function value transmitted by the interconnection bus supports storage in the register inside the point cloud rotation submodule, and the second input end of the second point cloud multiplier receives the sine function value transmitted by the interconnection bus through the connected register. Figure 3 In the embodiment shown, the second input terminal of the second point cloud multiplier is connected to the output terminal of the register SIN (the box marked with SIN), wherein the register SIN is used to cache the sine function value transmitted from the interconnection bus, and then transmit the currently cached sine function value to the second input terminal of the third point cloud multiplier. The third point cloud multiplier is used to control the multiplication of the vertical coordinate of the point cloud currently transmitted from the interconnection bus with the sine function value corresponding to the rotation angle achieved by the current rotation transformation, and then output the product value obtained by the multiplication. The third point cloud multiplier is used to control the multiplication of the horizontal coordinate of the point cloud received by the first input terminal of the third point cloud multiplier with the sine function value corresponding to the rotation angle achieved by the current rotation transformation received by the second input terminal of the third point cloud multiplier, and then output the product value obtained by the multiplication.

[0074] The first input terminal of the fourth point cloud multiplier is used to receive the vertical coordinate of the point cloud output by the second register, and the vertical coordinate of the point cloud output by the second register is the vertical coordinate of the point cloud currently received by the first input terminal of the second discrete adder. Figure 3 In the embodiment shown, the first input terminal of the fourth point cloud multiplier (at Figure 3 In the point cloud rotation submodule, the fourth multiplier arranged from top to bottom) and register P y (Marked with P y The output terminal of the block) is connected, wherein the register P yTherefore, the vertical coordinate of the point cloud transmitted by the interconnection bus to the first input terminal of the second discrete adder is first buffered in the second register and then output by the second register to the first input terminal of the fourth point cloud multiplier and the first input terminal of the second discrete adder.

[0075] The second input terminal of the fourth point cloud multiplier is used to receive the cosine function value corresponding to the rotation angle achieved by the current rotation transformation. Figure 3 In the illustrated embodiment, the second input of the fourth point cloud multiplier is connected to the output of register COS (box labeled COS), wherein register COS is used to cache the cosine function value transmitted from the interconnect bus and then transmit the currently cached cosine function value to the second input of the fourth point cloud multiplier. The fourth point cloud multiplier is used to control the multiplication of the vertical coordinate of the point cloud received at the first input of the fourth point cloud multiplier with the cosine function value corresponding to the rotation angle achieved by the current rotation transformation received at the second input of the fourth point cloud multiplier, and then output the product value.

[0076] The first input of the point cloud subtractor is used to receive the product value output by the first point cloud multiplier; the second input of the point cloud subtractor is used to receive the product value output by the second point cloud multiplier. In this embodiment, the first input of the point cloud subtractor is connected to the output of the first point cloud multiplier, and the second input of the point cloud subtractor is connected to the output of the second point cloud multiplier to construct a combinational logic circuit. The point cloud subtractor is used to control the subtraction of the product value output by the first point cloud multiplier and the product value output by the second point cloud multiplier, and then output the difference obtained by the subtraction and set it as the horizontal coordinate value of the rotated point cloud obtained after the point cloud undergoes the current rotation transformation. The point cloud subtractor is also used to transmit the output difference value to the memory module via the interconnection bus and overwrite the horizontal coordinate of the point cloud stored in the memory module, that is, to update the point cloud in the block storage space corresponding to the box marked with Discrete Point Cloud with the difference value output by the point cloud subtractor, so as to complete the update of the horizontal coordinate result of the current rotation transformation to the horizontal coordinate of the currently stored point cloud stored in the memory module.

[0077] The first input end of the point cloud adder is used to receive the product value output by the third point cloud multiplier; the second input end of the point cloud adder is used to receive the product value output by the fourth point cloud multiplier; in this embodiment, the first input end of the point cloud adder is connected to the output end of the third point cloud multiplier, and the second input end of the point cloud adder is connected to the output end of the fourth point cloud multiplier to construct a combinational logic circuit; the point cloud adder is used to control the addition of the product value output by the third point cloud multiplier and the product value output by the fourth point cloud multiplier, and then output the sum obtained by the addition, and set it as the vertical coordinate value of the rotated point cloud obtained after the point cloud undergoes the current rotation transformation; the point cloud adder is also used to transmit the output difference to the memory module through the interconnection bus and overwrite the vertical coordinate of the point cloud stored in the memory module, that is, to update the difference output by the point cloud subtractor to the aforementioned value marked with Discrete Point The point cloud in the block storage space corresponding to the box of Cloud is used to update the vertical coordinate result of the current rotation transformation to the vertical coordinate of the currently stored point cloud stored in the memory module.

[0078] It should be noted that the angle search step is stored in the search window parameter register, which is a parameter register set inside the correlation scan matching hardware circuit and is used to store the pose covered by the search window and the associated step search information; Figure 1 The parameter register set shown includes the search window parameter register. The number of rotations is determined by the pre-configured angular search range and pre-configured angular search step size of the search window, so that all poses within the search window can be traversed (queried) within the corresponding number of rotations. Each rotation corresponds to a coordinate system transformation performed by the point cloud processing module and is included in the map correlation scan matching process.

[0079] In summary, the embodiment of the point cloud rotation submodule uses the rotation matrix as the basic operation architecture, and only uses adders, subtractors and multipliers to construct the point cloud rotation submodule, which is used to receive and process the trigonometric function values ​​corresponding to the step rotation angles pre-calculated by the software. This not only eliminates the need for the trigonometric function hardware calculation module, but also controls the point cloud rotation submodule to iteratively perform the rotation transformation based on the step rotation, thereby obtaining a point cloud for updating the point cloud previously stored in the memory module. This technical solution combines the aforementioned point cloud discretization submodule to complete the traversal of each posture of the search window, so as to promote the TSDF value calculation module to construct a weighted function for each posture of the search window.

[0080] As an embodiment, the state machine control module is a finite state machine; the state machine control module is used to schedule the working states of the memory module, the point cloud processing module and the TSDF value calculation module, so that each time the point cloud processing module performs the coordinate system transformation and the rotation transformation once, the TSDF value calculation module performs the division operation once to obtain a TSDF value corresponding to a posture searched in the search window, so as to update the currently stored point cloud using the result obtained by the rotation transformation, control the point cloud processing module to continue to perform the coordinate system transformation, and control the TSDF value calculation module to perform another division operation until the TSDF value calculation module counts that all postures in the search window have been searched, and calculates the TSDF values ​​that match all postures in turn. In this embodiment, the state machine control module completes the correlation scan matching corresponding to the posture in the search window in a hardware-based scheduling working status signal, including controlling the point cloud processing module to perform rotation and translation transformation on each point cloud in turn, under the premise that the TSDF value calculation module calculates the TSDF value of a posture in the search window, so as to read the truncated signed distance function value and weight value matching the index value from the memory module, realize the operation of the loop state, ensure the normal operation of the hardware-based correlation scan matching, and accelerate the convergence speed of the algorithm.

[0081] In this embodiment, the correlation scan matching hardware circuit is controlled by a finite state machine to run the correlation scan matching algorithm to realize the calculation of the TSDF value after the point cloud is aligned to the grid map. The finite state machine includes 12 working states, namely: idle state, reading point cloud state, reading truncated signed distance function value state, reading weight value state, reading reset state, point cloud rasterization state, rasterization reset state, search state, division state, loop state, waiting for division completion state and write data state.

[0082] The initial state is the idle state, and the working state of the state machine control module jumps to the reading point cloud state. At this time, the state machine control module is used to first control the memory module to read the currently stored point cloud from the external FIFO, and then store the read point cloud in the first block of storage space, that is, the point cloud is transmitted from the external read FIFO to the block storage space corresponding to the box marked with PointCloud; if the point cloud of the currently collected circle is completed after reading, the state jumps to the reading truncated signed distance function value state. At this time, the memory module is controlled to read the truncated signed distance function value from the external read FIFO, and then the read truncated signed distance function value is stored in the second block of storage space, that is, the point cloud is transmitted from the external read FIFO to the block storage space marked with Map of tsdf i(x) in the block storage space corresponding to the box; when all the truncated signed distance function values ​​are read, the state jumps to the read weight value state, controls the memory module to read the weight value that matches the truncated signed distance function value from the external FIFO, and then stores the read weight value in the third block storage space, that is, the point cloud is transmitted from the external read FIFO to the block marked with Map of weight i After all weight values ​​are read into the third block storage space, the working state jumps to the read reset state, controls the read address of the memory module to reset, and controls the write address of the memory module to start self-incrementing, opening up a fourth block storage space so that the coordinate system transformation result output by the point cloud processing module is written into the empty block storage space in the memory module.

[0083] After the write address of the memory module is set by self-increment, the working state jumps to the point cloud rasterization state, controls the point cloud processing module to execute the coordinate system transformation, and then controls the result of the coordinate system transformation to be written into the fourth block of storage space via the interconnection bus. After the results of the current coordinate system transformation in the correlation scan matching of the map are written into the fourth block of storage space, the working state jumps to the rasterization reset state, controls the read address and write address of the first block of storage space to be reset, controls the read address of the fourth block of storage space to be reset, and controls the write address of the fourth block of storage space to start self-incrementing, so as to continue to open up empty address space in the fourth block of storage space for storing the results of the next coordinate system transformation; wherein, the point cloud participating in the next coordinate system transformation is the result of the rotation transformation obtained by the point cloud processing module while the point cloud processing module executes the current coordinate system transformation. After setting the address space for the result obtained by the next coordinate system transformation in the fourth block of storage space, the working state jumps to the search state, and controls the TSDF value calculation module to calculate the index value of the discrete point cloud mapped to the storage space of the truncated signed distance function value, and then reads the truncated signed distance function value and its matching weight value from the memory module according to the index value; after the calculation is completed, the working state jumps to the division state, and then according to the aforementioned embodiment, the division operation is performed once based on the read truncated signed distance function value and its matching weight value to obtain a TSDF value corresponding to a posture match searched in the search window, and then the TSDF value obtained by the operation is transmitted to the interconnection bus.

[0084] After completing the division calculation, the working state jumps to the loop state. At this time, the value of the loop register is set according to the coordinate index value of the current search posture in the search window. If it is determined that the search of all postures in the search window has not been completed, the working state is controlled to jump to the point cloud rasterization state, and the point cloud processing module is re-controlled to execute the coordinate system transformation to repeat the aforementioned working state. Therefore, each time the point cloud processing module executes the coordinate system transformation and the rotation transformation once, the TSDF value calculation module is controlled to execute the division operation once to obtain a TSDF value corresponding to a posture searched in the search window, and then the TSDF value obtained by the operation is transmitted to the interconnection bus.

[0085] If it is determined that the search for all poses within the search window has been completed, the working state jumps to the waiting for division completion state, at which time it waits for the last division to complete. After the last division is completed, the working state jumps to the writing data state, that is, the TSDF value calculation module counts that all existing poses within the search window have been searched, and then writes all TSDF values ​​obtained by calculation to the external write FIFO via the interconnect bus. When the data writing is completed, the working state jumps back to the idle state, waiting for the next acceleration calculation to begin.

[0086] As an embodiment, a bus interface module is provided outside the correlation scan matching hardware circuit, and the bus interface module includes a DMA controller module and a transmission bus; preferably, the bus interface module includes an AXI DMA module and an AXI bus.

[0087] The DMA controller module is used to continuously transfer data stored in physical storage spaces with discontinuous addresses in batches, thereby reducing the number of times the CPU software interrupt is triggered. Specifically, the DMA controller module requires the CPU device driver to generate a linked list of storage data addresses, use the linked list to describe the discontinuous physical space, and send the initial address of the linked list to the DMA controller module. After the DMA controller module transmits a section of data to the correlation scan matching hardware circuit, it reads the address of the next linked list in sequence until all data is transmitted.

[0088] The transmission bus includes a first bus and a second bus. Preferably, the transmission bus is an AXI bus, including two types of AXI buses: AXI-Lite bus and AXI-Stream bus. The former is suitable for simple, low-throughput memory-mapped communication, and the latter is for high-speed streaming data. The first bus is preferably an AXI-Lite bus, which connects the processor unit to the correlation scan matching hardware circuit and the DMA controller module, and is capable of sending control signals to the memory module, the point cloud processing module, the TSDF value calculation module, the state machine control module, and the interconnect bus, and monitoring the operating status of the state machine control module. The first bus is in signal transmission and reception communication with the memory module, the point cloud processing module, the TSDF value calculation module, the state machine control module, the interconnect bus, and the DMA controller module. The first bus is used to configure data transmission parameters for the DMA controller module. The first bus is also used to configure the parameters stored in the map size register, the parameters stored in the map resolution register, the extended parameters required for the maximum detection radius of the currently stored point cloud, the extended parameters required for performing the coordinate system transformation, the extended parameters required for performing the rotation transformation, the extended parameters required to meet the transmission requirements of the bus bit width, and the search step size, so as to achieve memory mapping communication of the correlation scan matching hardware circuit. The second bus is preferably an AXI-Stream bus, which is connected to the DMA controller module and is used to transmit the truncated signed distance function and its matching weight value pre-configured by the CPU to the memory module. The transmission bus complies with the AMBA protocol. This embodiment provides a bus interface architecture module for the correlation scan matching hardware circuit and an external data source (controller). According to the data transmission performance, a first bus suitable for simple, low-throughput memory mapping communication (transmitting extended parameters, map feature parameters and associated basic control signals to the registers inside the correlation scan matching hardware circuit) and a second bus for high-speed data flow (transmitting truncated signed distance function values ​​and weight values ​​to the memory module) are designed respectively; thereby improving the real-time performance of the calculation work of the correlation scan matching hardware circuit.

[0089] Based on the aforementioned embodiments, the present invention further discloses a heterogeneous computing architecture, comprising a processor unit, a bus interface, and the correlation scan matching hardware circuit disclosed in the aforementioned embodiments. The processor unit and the correlation scan matching hardware circuit are interconnected via the bus interface. The processor unit does not control the parallel acceleration operations performed by the correlation scan matching hardware circuit. The processor unit only provides operation parameters, start signals, interrupt signals, reset signals, and flag clear signals to the correlation scan matching hardware circuit via the bus interface. When the processor unit is a control circuit board independent of the correlation scan matching hardware circuit, the heterogeneous computing architecture is applied to a heterogeneous computing circuit system. When the processor unit, the bus interface, and the correlation scan matching hardware circuit are integrated on the same chip, the heterogeneous computing architecture is applied to a heterogeneous chip. The heterogeneous architecture strikes a balance between accuracy and cost. The correlation scan matching hardware circuit is suitable for data parallel algorithms, so that the acceleration obtained by the heterogeneous architecture promotes real-time processing of high-frame-rate point cloud maps.

[0090] The above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention in any other manner. Any person skilled in the art may utilize the above-disclosed technical content to modify or modify the present invention into equivalent embodiments. However, any simple modifications, equivalent variations, and modifications to the above embodiments that do not depart from the technical content of the present invention and are based on the technical essence of the present invention remain within the scope of protection of the present invention.

Claims

1. The correlation scan matching hardware circuit of TSDF map is characterized by: The correlation scan matching hardware circuit is electrically connected to the laser radar; The correlation scan matching hardware circuit includes a memory module, a point cloud processing module, a TSDF value calculation module, a state machine control module and an interconnection bus, wherein the memory module, the point cloud processing module, the TSDF value calculation module and the state machine control module all establish a data transmission relationship through the interconnection bus; The point cloud processing module is used to read the currently stored point cloud from the memory module under the control of the state machine control module, then control the read point cloud to perform coordinate system transformation, and set the result of the coordinate system transformation as a discrete point cloud to align the currently stored point cloud to the coordinate system of the pre-expanded grid map; and then store the result of the coordinate system transformation into the memory module; The point cloud processing module is further configured to, under the control of the state machine control module, read the currently stored point cloud from the memory module, control the read point cloud to perform a rotation transformation, then update the result of the rotation transformation to the currently stored point cloud stored in the memory module, and control the updated point cloud to perform the aforementioned coordinate system transformation to determine the completion of one matching process of the correlation scan matching of the map; wherein each matching process of the correlation scan matching of the map includes one coordinate system transformation; The TSDF value calculation module is used to read the discrete point cloud from the memory module under the control of the state machine control module, and then calculate the index value of the discrete point cloud mapped to the storage space of the truncated signed distance function value according to the preset coordinate offset value and search step size, and set the index value to the read address of the truncated signed distance function value and the read address of the weight value stored in the memory module; wherein the truncated signed distance function value and the weight value both have a matching index value; The TSDF value calculation module is also used to read the truncated signed distance function value and its matching weight value from the memory module according to the currently set read address under the control of the state machine control module, and then control the built-in divider of the TSDF value calculation module to equivalently complete the weighted average processing of the truncated signed distance function value mapped by the currently stored point cloud by performing a division operation, and set the result of each operation output of the divider as the TSDF value, which is used to characterize the positioning accuracy of a corresponding search posture in the search window; wherein, each time the point cloud processing module completes a matching process of the correlation scan matching of the map, the built-in divider of the TSDF value calculation module performs a division operation to obtain a TSDF value corresponding to a new posture in the search window; The memory module is used to store the currently stored point cloud, the discrete point cloud obtained by transforming the currently stored point cloud through the point cloud processing module, the pre-set truncated signed distance function and its matching weight value; wherein each type of data is stored in different block storage spaces of the same memory module.

2. The correlation scan matching hardware circuit according to claim 1, characterized in that: The TSDF value calculation module includes a selector, a frequency division counter, a pre-calculation unit, a divider, a first accumulator, a second accumulator, and a third accumulator; The TSDF value calculation module is configured to, under the control of the state machine control module, send the truncated signed distance function value into a first accumulator, send the weight value into a second accumulator, and send the truncated signed distance function value and the weight value matched thereto into a third accumulator, and control each accumulator to perform an accumulation operation simultaneously; wherein the input ends of the three accumulators are configured to receive data sequentially transmitted from the interconnection bus, and the third accumulator is configured to output a cumulative sum value of the product of the truncated signed distance function value and the weight value matched thereto; The output terminals of the three accumulators are respectively connected to corresponding data input terminals of the selector, and the select input terminal of the selector is connected to the count output terminal of the frequency division counter. The selector is provided with a data output terminal matching each data input terminal, and is used to select and output the data received by the data input terminal of the selector under the counting trigger of the frequency division counter to adapt to the enable timing of the divider; Each data output terminal of the selector is connected to a corresponding input terminal of the pre-calculation unit; The pre-calculation unit includes a numerator adder, a first numerator subtractor, a second numerator subtractor, a numerator shift register, and a denominator shift register; the numerator adder is an adder, the first numerator subtractor and the second numerator subtractor are subtractors, and the numerator shift register and the denominator shift register are shift registers; wherein a first input end of the second numerator subtractor is connected to an output end of the first accumulator, and a second input end of the second numerator subtractor is connected to an output end of the third accumulator; the second numerator subtractor is used to control the subtraction of the accumulated sum output by the first accumulator and the accumulated sum output by the third accumulator, and then output the difference between the two subtracted data; The first numerator subtractor is provided with an input terminal for receiving the quantity value of the currently stored point cloud transmitted by the interconnection bus; the first numerator subtractor is provided with another input terminal connected to the output terminal of the second accumulator; the first numerator subtractor is used to control the subtraction of the accumulated sum value output by the second accumulator and the quantity value of the currently stored point cloud transmitted by the interconnection bus, and then output the difference between the two subtracted data; the input terminal of the numerator shift register is connected to the output terminal of the first numerator subtractor, and the numerator shift register is used to shift the difference value output by the first numerator subtractor so that the bit width of the binary number obtained by the shifted difference value output by the first numerator subtractor is equal to the bit width of the accumulated sum value output by the first accumulator; The two input ends of the numerator adder are respectively connected to the output end of the numerator shift register and the output end of the second numerator subtractor. The numerator adder is used to add the shift output result of the numerator shift register to the difference output by the second numerator subtractor, and then configure the sum of the two added data as the dividend; An input end of the denominator shift register is connected to an output end of the first numerator subtractor, and the denominator shift register is used to shift the difference value output by the first numerator subtractor so that a binary digit width obtained by shifting the difference value output by the first numerator subtractor is one bit lower than a binary digit width of the accumulated sum value output by the first accumulator; The two input ends of the divider are respectively connected to the output end of the numerator adder and the output end of the denominator shift register. The divider is used to control the dividend output by the numerator adder and the divisor output by the denominator shift register to perform a division operation after being enabled, and then send the calculated quotient value back to the interconnection bus to determine that the quotient value becomes the TSDF value corresponding to a posture within the search window, and also determine that the divider completes a weighted average processing of the truncated signed distance function value mapped by the currently stored point cloud by performing a division operation.

3. The correlation scan matching hardware circuit according to claim 1, characterized in that: The TSDF value calculation module includes a grid index submodule, and the grid index submodule includes a first index subtractor, a second index subtractor, a first index adder, a second index adder, a third index adder, and an index multiplier; wherein the first index subtractor and the second index subtractor are both subtractors, and the first index adder and the second index adder are both adders; The first input end of the first index subtractor is used to receive the horizontal coordinate of the discrete point cloud transmitted by the interconnection bus, wherein the horizontal coordinate of the discrete point cloud transmitted to the first index subtractor by the interconnection bus is derived from the memory module; The second input terminal of the first index subtractor is used to receive a horizontal axis coordinate offset value transmitted from the interconnection bus, wherein the preset coordinate offset value includes a horizontal axis coordinate offset value, and the horizontal axis coordinate offset value transmitted from the interconnection bus is derived from the preset coordinate offset value stored in a map offset value register; the map offset value register is a parameter register set within the correlation scan matching hardware circuit, and is used to store the coordinate offset value associated with the pre-expanded grid map; A first input end of the first index adder is connected to an output end of the first index subtractor; a second input end of the first index adder is used to receive a horizontal axis coordinate search step length transmitted from the interconnection bus; wherein the search step length includes a horizontal axis coordinate search step length, and the horizontal axis coordinate search step length transmitted from the interconnection bus is derived from the search step length stored in the search window parameter register; the search window parameter register is a parameter register set inside the correlation scan matching hardware circuit, and is used to store the posture covered by the search window and associated search information, including the search step length; the sum value output by the first index adder is configured as a horizontal axis direction index value in the storage space where the discrete point cloud is mapped to the truncated signed distance function value; The first input end of the second index subtractor is used to receive the vertical coordinate of the discrete point cloud transmitted by the interconnection bus, wherein the vertical coordinate of the discrete point cloud transmitted to the second index subtractor by the interconnection bus is derived from the memory module; The second input end of the second index subtractor is used to receive the vertical axis coordinate offset value transmitted by the interconnection bus, wherein the preset coordinate offset value also includes the vertical axis coordinate offset value, and the vertical axis coordinate offset value transmitted by the interconnection bus is also derived from the preset coordinate offset value stored in the map offset value register; The first input end of the second index adder is connected to the output end of the second index subtractor; the second input end of the second index adder is used to receive the vertical axis coordinate search step transmitted by the interconnection bus; wherein, the search step also includes the vertical axis coordinate search step, and the vertical axis coordinate search step transmitted by the interconnection bus is also derived from the search step stored in the search window parameter register; the sum value output by the second index adder is configured as the vertical axis direction index value in the storage space where the discrete point cloud is mapped to the truncated signed distance function value.

4. The correlation scan matching hardware circuit according to claim 3, characterized in that: The grid index submodule further includes a third index adder and an index multiplier, the third index adder belongs to the adder, and the index multiplier belongs to the multiplier; A first input terminal of the index multiplier is connected to an output terminal of the second index adder; a second input terminal of the index multiplier is used to receive a row grid number transmitted from the interconnection bus, wherein the row grid number is the number of grids that can be occupied by each row of the storage space of the truncated signed distance function value, is stored in a map size register, and is transmitted to the interconnection bus by the map size register; the map size register is a parameter register set within the correlation scan matching hardware circuit, and is used to store the size range of the pre-expanded grid map and associated expansion information; The first input end of the third index adder is connected to the output end of the first index adder, and the second input end of the third index adder is connected to the output end of the index multiplier. The third index adder is used to control the product of the number of row grids and the sum value output by the second index adder and add the horizontal axis index value, and then set the sum value obtained by the addition as the index value in the storage space of the discrete point cloud mapped to the truncated signed distance function value, and send it to the interconnection bus, so as to read the truncated signed distance function value and the weight value matching the index value from the memory module.

5. The correlation scan matching hardware circuit according to claim 3, characterized in that: The grid index submodule further includes a third index adder and an index multiplier, the third index adder belongs to the adder, and the index multiplier belongs to the multiplier; A first input terminal of the index multiplier is connected to an output terminal of the first index adder; a second input terminal of the index multiplier is used to receive the number of column grids transmitted from the interconnection bus; wherein the number of column grids is the number of grids that can be occupied by each column of the storage space of the truncated signed distance function value, stored in a map size register, and transmitted to the interconnection bus by the map size register; the map size register is a parameter register set within the correlation scan matching hardware circuit, and is used to store the size range of the pre-expanded grid map and associated expansion information; The first input end of the third index adder is connected to the output end of the second index adder, and the second input end of the third index adder is connected to the output end of the index multiplier; the third index adder is used to control the product of the number of column grids and the sum value output by the first index adder and add the vertical axis index value, and then set the sum value obtained by the addition as the index value in the storage space of the discrete point cloud mapped to the truncated signed distance function value, and send it to the interconnection bus, so as to facilitate reading the truncated signed distance function value and the weight value matching the index value from the memory module.

6. The correlation scan matching hardware circuit according to claim 1, characterized in that: The pre-expanded grid map is a grid map obtained by expanding the original point cloud delineated map, sequentially according to the expansion parameters required for the maximum detection radius reached by the currently stored point cloud, the expansion parameters required for performing the coordinate system transformation, the expansion parameters required for performing the rotation transformation, and the expansion parameters required for aligning the bus width; The original point cloud is the total laser point cloud obtained by the current scan of the laser radar. The map defined by the original point cloud is a grid map centered on the laser radar and defined by the maximum detection diameter achieved in the original point cloud as the side length of the map. The alignment bus bit width means that the memory of the grid covered by the side length of the pre-expanded grid map is equal to the bit width of the bus transmitting the grid map. Among them, the preset coordinate offset value is determined by the sum of the coordinate translation parameters required for the maximum detection radius reached by the currently stored point cloud, the coordinate translation parameters required to perform the coordinate system transformation, the coordinate translation parameters required to perform the rotation transformation and the coordinate translation parameters required to align the bus width.

7. The correlation scan matching hardware circuit according to claim 6, characterized in that: The point cloud processing module includes a point cloud discretization submodule; The point cloud discretization submodule includes a first discrete adder, a first discrete subtractor, a first discrete multiplier, a second discrete adder, a second discrete subtractor and a second discrete multiplier. The first discrete adder and the second discrete adder are both adders, the first discrete subtractor and the second discrete subtractor are both subtractors, and the first discrete multiplier and the second discrete multiplier are both multipliers. a first input end of the first discrete adder, configured to receive a horizontal coordinate of a point cloud transmitted by the interconnection bus, wherein the point cloud transmitted by the interconnection bus to the first discrete adder is derived from the currently stored point cloud stored in the memory module; and the horizontal coordinate of the point cloud is a horizontal coordinate value in a laser coordinate system; The second input terminal of the first discrete adder is used to receive the horizontal coordinate of the robot position transmitted by the interconnection bus, wherein the horizontal coordinate of the robot position is a pre-calculated horizontal axis coordinate value of the robot in the world coordinate system; A first discrete adder is used to control the addition of the horizontal coordinate of the point cloud and the horizontal coordinate of the robot position, and then output the sum value so that the sum value becomes the horizontal axis coordinate value of the point cloud converted to the world coordinate system; A first input of the first discrete subtractor is connected to an output of the first discrete adder; a second input of the first discrete subtractor is configured to receive a maximum horizontal coordinate value of the map transmitted via the interconnection bus, the maximum horizontal coordinate value of the map being derived from a map size register and transmitted to the interconnection bus by the map size register; the map size register being a parameter register internally provided within the correlation scan matching hardware circuit and configured to store a size range of the grid map that meets the bus bit width transmission requirement; and a difference value output by the first discrete subtractor becomes the vertical coordinate value of the grid map that meets the bus bit width transmission requirement. The first input end of the first discrete multiplier is connected to the output end of the first discrete subtractor; the second input end of the first discrete multiplier is used to receive the inverse of the map resolution transmitted from the interconnection bus, the inverse of the map resolution is derived from the map resolution register and transmitted to the interconnection bus by the map resolution register; wherein the map resolution register is a parameter register set inside the correlation scan matching hardware circuit, and is used to store the resolution information of the raster map that meets the transmission requirements of the bus bit width; the product value output by the first discrete multiplier is configured as the horizontal coordinate value of the discrete point cloud to complete the coordinate system transformation and realize the alignment of the horizontal coordinate value of the currently stored point cloud to the coordinate system of the raster map that meets the transmission requirements of the bus bit width; the first discrete multiplier is further used to output the product value obtained by multiplication to the memory module through the interconnection bus, so that the TSDF value calculation module can read the horizontal coordinate value of the discrete point cloud; a first input terminal of the second discrete adder, configured to receive a vertical coordinate of a point cloud transmitted by the interconnection bus, wherein the point cloud transmitted by the interconnection bus to the second discrete adder is derived from the currently stored point cloud stored in the memory module; and the vertical coordinate of the point cloud is a vertical coordinate value in a laser coordinate system; A second input end of the second discrete adder is configured to receive the vertical coordinate of the robot position transmitted by the interconnection bus, wherein the vertical coordinate of the robot position is a pre-calculated vertical axis coordinate value of the robot in the world coordinate system; The second discrete adder is used to control the addition of the vertical coordinate of the point cloud and the vertical coordinate of the robot position, and then output the sum value so that the sum value becomes the vertical axis coordinate value of the point cloud converted to the world coordinate system; A first input terminal of the second discrete subtractor is connected to an output terminal of the second discrete adder; a second input terminal of the second discrete subtractor is used to receive a maximum vertical coordinate value of the map transmitted from the interconnection bus, wherein the maximum vertical coordinate value of the map is derived from the map size register and transmitted to the interconnection bus by the map size register; a difference value output by the second discrete subtractor becomes the horizontal coordinate value of the grid map that meets the transmission requirement of the bus bit width; The first input end of the second discrete multiplier is connected to the output end of the second discrete subtractor; the second input end of the second discrete multiplier is used to receive the inverse of the map resolution transmitted by the interconnection bus; the product value output by the second discrete multiplier is configured as the vertical coordinate value of the discrete point cloud to complete the coordinate system transformation and realize the alignment of the vertical coordinate value of the currently stored point cloud to the coordinate system of the raster map that meets the transmission requirements of the bus bit width; the second discrete multiplier is also used to output the product value obtained by multiplication to the memory module through the interconnection bus, so that the TSDF value calculation module can read the vertical coordinate value of the discrete point cloud.

8. The correlation scan matching hardware circuit according to claim 7, characterized in that: The point cloud processing module also includes a point cloud rotation submodule; The point cloud rotation submodule includes a first point cloud multiplier, a second point cloud multiplier, a third point cloud multiplier, a fourth point cloud multiplier, a point cloud adder, and a point cloud subtractor. The first point cloud multiplier, the second point cloud multiplier, the third point cloud multiplier, and the fourth point cloud multiplier are all multipliers, the point cloud adder is an adder, and the point cloud subtractor is a subtractor. The first input terminal of the first point cloud multiplier is connected to the output terminal of the first register; wherein the first register is provided inside the point cloud discretization submodule; the horizontal coordinate of the point cloud transmitted by the interconnection bus to the first input terminal of the first discrete adder is cached in the first register; a second input terminal of the first point cloud multiplier, configured to receive a cosine function value corresponding to a rotation angle achieved by a current rotation transformation, wherein the rotation angle achieved by the current rotation transformation is a sum of an angle search step and a rotation angle achieved by a previous rotation transformation; the angle search step is stored in a search window parameter register; the search window parameter register is a parameter register set within the correlation scan matching hardware circuit, configured to store a pose covered by the search window and associated step search information; The first input terminal of the second point cloud multiplier is connected to the output terminal of the first register; wherein the second register is provided inside the point cloud discretization submodule; the vertical coordinate of the point cloud transmitted by the interconnection bus to the first input terminal of the second discrete adder is cached in the second register; The second input terminal of the second point cloud multiplier is used to receive the sine function value corresponding to the rotation angle achieved by the current rotation transformation; A first input terminal of a third point cloud multiplier is connected to an output terminal of the first register; The second input terminal of the third point cloud multiplier is used to receive the sine function value corresponding to the rotation angle achieved by the current rotation transformation; A first input terminal of the fourth point cloud multiplier is connected to an output terminal of the second register; The second input terminal of the fourth point cloud multiplier is used to receive the cosine function value corresponding to the rotation angle achieved by the current rotation transformation; A first input terminal of the point cloud subtractor is connected to an output terminal of the first point cloud multiplier, and a second input terminal of the point cloud subtractor is connected to an output terminal of the second point cloud multiplier, wherein a difference value output by the point cloud subtractor is set as a horizontal coordinate value of the rotated point cloud obtained after the point cloud undergoes a current rotation transformation; the point cloud subtractor is further configured to transmit the output difference value to the memory module via the interconnection bus and overwrite the horizontal coordinate of the point cloud stored in the memory module, thereby completing the update of the horizontal coordinate result obtained by the current rotation transformation to the horizontal coordinate of the currently stored point cloud stored in the memory module; A first input terminal of the point cloud adder is connected to an output terminal of the third point cloud multiplier, and a second input terminal of the point cloud adder is connected to an output terminal of the fourth point cloud multiplier, wherein a sum value output by the point cloud adder is set as a vertical coordinate value of the rotated point cloud obtained after the point cloud undergoes a current rotation transformation; the point cloud adder is further configured to transmit the output difference value to the memory module via the interconnection bus and overwrite the vertical coordinate of the point cloud stored in the memory module, thereby completing the updating of the vertical coordinate result obtained by the current rotation transformation to the vertical coordinate of the currently stored point cloud stored in the memory module; The number of rotations is determined by the pre-configured angular search range of the search window and the pre-configured angular search step size; each rotation transformation corresponds to a coordinate system transformation performed by the point cloud processing module and belongs to a matching process of the correlation scan matching of the map.

9. The correlation scan matching hardware circuit according to any one of claims 1 to 8, characterized in that: The state machine control module is a finite state machine; the state machine control module is used to schedule the working states of the memory module, the point cloud processing module and the TSDF value calculation module, so that each time the point cloud processing module performs the coordinate system transformation and the rotation transformation once, the TSDF value calculation module performs the division operation once to obtain a TSDF value corresponding to a posture searched in the search window, and on the basis of updating the currently stored point cloud with the result obtained by the rotation transformation, controls the point cloud processing module to continue to perform the coordinate system transformation, and controls the TSDF value calculation module to perform the division operation again, until the TSDF value calculation module counts that all postures in the search window have been searched, and calculates the TSDF values ​​that match all postures in turn; The search step size is the number of grid searches that need to be traversed each time a posture is searched within the same search window; the search window is a neighborhood grid area centered on a point cloud within the pre-expanded grid map.

10. The correlation scan matching hardware circuit according to claim 8, characterized in that: A bus interface module is provided outside the correlation scan matching hardware circuit, and the bus interface module includes a DMA controller module and a transmission bus; The DMA controller module is used to continuously transfer data stored in the physical storage space of discontinuous addresses in batches, reducing the number of times the CPU software interrupt is triggered; The transmission bus includes a first bus and a second bus. The first bus is in signal transmission and reception communication with the memory module, the point cloud processing module, the TSDF value calculation module, the state machine control module, the interconnection bus, and the DMA controller module, respectively. The first bus is used to configure data transmission parameters for the DMA controller module. The first bus is also used to configure the parameters stored in the map size register, the parameters stored in the map resolution register, the extended parameters required for the maximum detection radius reached by the currently stored point cloud, the extended parameters required for performing the coordinate system transformation, the extended parameters required for performing the rotation transformation, the extended parameters required to meet the transmission requirements of the bus bit width, and the search step size, so as to realize mapping communication of the memory of the correlation scan matching hardware circuit; The second bus is connected to the DMA controller module and is used to transmit the truncated signed distance function and its matching weight value pre-configured by the CPU to the memory module; Wherein, the transmission bus complies with the AMBA protocol.

11. A heterogeneous computing architecture, characterized in that: A heterogeneous computing architecture comprising a processor unit, a bus interface, and the correlation scan matching hardware circuit according to any one of claims 1 to 9, wherein the processor unit and the correlation scan matching hardware circuit are interconnected via the bus interface, the processor unit does not control the parallel acceleration operation performed by the correlation scan matching hardware circuit, and the processor unit only provides operation parameters, start signals, interrupt signals, reset signals, and flag clear signals to the correlation scan matching hardware circuit via the bus interface; Wherein, when the processor unit is a control circuit board independent of the correlation scan matching hardware circuit, the heterogeneous computing architecture is applied to a heterogeneous computing circuit system; When the processor unit, the bus interface and the correlation scan matching hardware circuit are integrated on the same chip, the heterogeneous computing architecture is applied to a heterogeneous chip.

Images