CN115984344A - Correlation scanning matching hardware circuit of TSDF (time series distribution function) map and heterogeneous computing framework - Google Patents

Abstract

The invention discloses a correlation scanning matching hardware circuit and a heterogeneous computing framework of a TSDF (time series distribution function) map, wherein the correlation scanning matching hardware circuit comprises a memory module, a point cloud processing module, a TSDF value computing module, a state machine control module and an interconnection bus, wherein the memory module, the point cloud processing module, the TSDF value computing module and the state machine control module establish a data transmission relation through the interconnection bus; the state machine control module is used for scheduling the working states of the memory module, the point cloud processing module and the TSDF value calculating module, so that when the point cloud processing module executes the coordinate system transformation and the rotation transformation once, the TSDF value calculating module executes the division operation once to obtain a TSDF value which is matched with one corresponding searched pose in the search window until the TSDF value calculating module counts all the poses in the search window.

Description

Correlation scanning matching hardware circuit and heterogeneous computing architecture of TSDF (three-dimensional transform function) map

Technical Field

The invention relates to the technical field of hardware acceleration, in particular to a correlation scanning matching hardware circuit based on a TSDF (time delay and distortion) map and a heterogeneous computing framework.

Background

A scene of dimension d may be represented using voxels (voxels) of the same size of dimension d, e.g., in a three-dimensional scene, a voxel grid is a data structure that represents three-dimensional objects with fixed-size cubic blocks as the smallest units; for a two-dimensional scene, the voxel is a two-dimensional grid, the location of which is defined by its central coordinate. The voxels may be viewed as point clouds in a point cloud map.

When using the sign distance function sdf (x) for scene reconstruction, each voxel contains two values, namely a sign distance function value and a weight value. The symbolic distance function value refers to the signed distance from the voxel center to the nearest object surface in the real scene, i.e. represents the relative distance between the voxel center and the nearest object surface in the current measurement direction. When the voxel center is located before the object surface (not into the object), the signed distance function value is defined as a positive number; the signed distance function value is defined as a negative number when the voxel center is located behind (into) the object surface, or on the side occluded by the object surface. The weight values contained by a voxel are used to evaluate the measurement uncertainty of the symbol distance function value, i.e. to represent the expected uncertainty of the voxel for the corresponding symbol distance function value in the current measurement direction (line of sight of the symbol distance pointing from the center of the voxel to the surface).

When the symbol distance function sdf (x) is truncated at a preset truncation distance t, a truncated symbol distance function tsdf (x) is defined, and as a truncation of the symbol distance function sdf (x), the symbol distance function sdf (x) is equivalently mapped into a truncated symbol distance field, for example, in a three-dimensional space with a specific size, the obstacle cube to be detected is subdivided into small three-dimensional voxel grids, and each voxel stores its quantized distance to the surface of the real scene through the truncated symbol distance function tsdf (x) (the symbol distance function values in the range of ± t of the truncation distance are mapped in a preset coding mode).

Currently the TSDF value is denoted as TSDF _i (x) The integral of (b) can be obtained by calculation in a weighted summation manner or a weight increment updating manner, where i is the number of currently stored point clouds and the number of voxels; the TSDF value may be obtained by mapping a point cloud corresponding to a laser data frame into a voxel through a symbol distance function calculation, and a truncated symbol distance function TSDF (x) contained in each voxel and a weight value matched with the truncated symbol distance function TSDF (x) are used as coordinate values of the voxel in a global coordinate system, and are configured as a two-dimensional grid map based on TSDF (hereinafter, referred to as TSDF map). Particularly in the field of SLAM of mobile robots, the TSDF map is more suitable for being combined with SLAM algorithm relative to an occupancy grid map to execute map navigation and positioning operation of the robots, however, the constructed TSDF map has a significant defect: TSDF maps will consume more storage space than do grid maps.

Moreover, at present, one of the most challenging research subjects in the SLAM algorithm of the laser radar-based mobile robot is to use a scan matching algorithm to realize accurate positioning of the robot. The Correlation Scan Matching (CSM) algorithm is one of the most widely used scan matching algorithms at present. Its advantages are moderate calculation cost and less accumulated error. The Correlation Scan Matching (CSM) algorithm is often optimized based on a priori estimation, and in a search window with a limited size, the currently acquired radar point cloud is rotated and translated to complete the operation of aligning the map. However, the CSM algorithm has one significant drawback: the computing complexity is extremely high, and the method is difficult to be deployed on an embedded computing platform with limited computing power.

Disclosure of Invention

In order to solve the problem that a large amount of memory space is consumed by a TSDF map and a correlation scanning matching algorithm used for the map disclosed by the prior art, the invention discloses a correlation scanning matching hardware circuit and a heterogeneous computing framework based on the TSDF map, and the specific technical scheme is as follows:

the correlation scanning and matching hardware circuit of the TSDF map has an electrical connection relation with the laser radar; the correlation scanning matching hardware circuit comprises a memory module, a point cloud processing module, a TSDF value calculating module, a state machine control module and an interconnection bus, wherein the memory module, the point cloud processing module, the TSDF value calculating module and the state machine control module establish a data transmission relation through the interconnection bus; the point cloud processing module is used for reading the currently stored point cloud from the memory module under the control of the state machine control module, controlling the read point cloud to execute coordinate system transformation, setting the result of the coordinate system transformation as discrete point cloud, and aligning the currently stored point cloud into the coordinate system of the pre-expanded grid map; then storing the result of the coordinate system transformation into a memory module; the point cloud processing module is also used for reading the currently stored point cloud from the memory module under the control of the state machine control module, controlling the read point cloud to execute rotation transformation, updating the result of the rotation transformation into the currently stored point cloud stored in the memory module, controlling the updated point cloud to execute the coordinate system transformation and determining the primary matching processing for finishing the correlation scanning matching of the map; each matching process of the correlation scanning matching of the map comprises a rotation transformation and a coordinate system transformation; the TSDF value calculating module is used for reading the discrete point cloud from the memory module under the control of the state machine control module, calculating an index value in a storage space of the discrete point cloud mapped to the truncation symbol distance function value according to a preset coordinate offset value and a search step length, and setting the index value as a reading address of the truncation symbol distance function value and a reading address of the weight value stored in the memory module; the truncation symbol distance function value and the weight value have a matched index value; the search step length is the number of raster searches which need to be traversed for searching one pose each time in the same search window; the search window is a neighborhood grid region which takes a point cloud as a center in the pre-expanded grid map; the TSDF value calculating module is also used for reading a truncated symbol distance function value and a matched weight value thereof from the memory module according to the currently set reading address under the control of the state machine control module, then controlling a divider arranged in the TSDF value calculating module to equivalently complete the weighted average processing of the truncated symbol distance function value mapped by the currently stored point cloud in a division operation mode, and setting the result output by each operation of the divider as a TSDF value for the positioning precision of a corresponding pose searched in the representation search window; when the point cloud processing module finishes one matching process of the correlation scan matching of the map, a divider built in the TSDF value calculating module executes one division operation to obtain a TSDF value corresponding to a new pose in a search window; the memory module is used for storing the currently stored point cloud, the discrete point cloud obtained by the currently stored point cloud through the transformation of the point cloud processing module, a preset truncation symbol distance function and a matched weight value of the function; wherein each type of data is stored in a different block of storage space of the same memory module.

According to the technical scheme, aiming at each pose in a search window in a pre-expanded grid map, a TSDF value calculation module is called to execute division operation once to obtain a corresponding TSDF value on the pose, the division operation is scheduled and executed through a state machine control module on the basis of a discrete point cloud provided by a point cloud processing module and a result for updating the rotation transformation of the discrete point cloud provided by the point cloud processing module, and meanwhile, the associated operation result and the transformation result are guaranteed to be shared on an interconnection bus; compared with the prior art, the hardware operation mode of the technical scheme reduces the division operation amount in hardware, does not increase the extra storage space of the hardware, and reduces the consumption of a large amount of programmable logic resources.

Further, the TSDF value calculating module comprises a selector, a frequency division counter, a pre-calculating unit, a divider, a first accumulator, a second accumulator and a third accumulator; the TSDF value calculating module is used for sending the truncated symbol distance function value to a first accumulator and sending the weight value to a second accumulator under the control of the state machine control module, sending the truncated symbol distance function value and the weight value matched with the truncated symbol distance function value to a third accumulator, and controlling each accumulator to simultaneously execute accumulation operation; the input ends of the three accumulators are configured to receive data sequentially transmitted by the interconnection bus, and the third accumulator is used for outputting the accumulated sum of the products of the truncated symbol distance function values and the matched weight values; the output ends of the three accumulators are respectively connected with the corresponding data input ends of the selector, the selection input end of the selector is connected with the counting output end of the frequency division counter, and the selector is provided with a data output end matched with each data input end and used for gating and outputting data received by the data input end of the selector under the counting triggering action of the frequency division counter so as to adapt to the enabling time sequence of the divider; each data output end of the selector is respectively connected with the corresponding input end of the pre-calculation unit; the pre-calculation unit comprises a numerator adder, a first numerator subtracter, a second numerator subtracter, a numerator shift register, a first numerator subtracter and a denominator shift register; the numerator adder belongs to the adder, the first numerator subtracter and the second numerator subtracter both belong to subtractors, and the numerator shift register and the denominator shift register both belong to shift registers; the first input end of the second sub-subtracter is connected with the output end of the first accumulator, and the second input end of the second sub-subtracter is connected with the output end of the third accumulator; the second sub-subtracter is used for controlling the accumulated sum value output by the first accumulator to be subtracted from the accumulated sum value output by the third accumulator and outputting the difference value of the two subtracted data; the first molecular subtracter is provided with an input end for receiving the quantity value of the currently stored point cloud transmitted by the interconnection bus; the first molecular subtracter is provided with the other input end connected with the output end of the second accumulator; the first molecular subtracter is used for controlling 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 outputting the difference value of the two subtracted data; the input end of the molecular shift register is connected with the output end of the first molecular subtracter, and the molecular shift register is used for shifting the difference value output by the first molecular subtracter, so that the bit width of a binary number obtained by shifting the difference value output by the first molecular subtracter is equal to the bit width of an accumulated sum value output by the first accumulator; the two input ends of the molecular adder are respectively connected with the output end of the molecular shift register and the output end of the second molecular subtracter, and the molecular adder is used for adding the shift output result of the molecular shift register to the difference value output by the second molecular subtracter and then configuring the sum value of the two added data into a dividend; the first sub-subtracter is used for controlling 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 outputting the difference value of the two subtracted data; the input end of the denominator shift register is connected with the output end of the first sub-subtracter, and the denominator shift register is used for shifting the difference value output by the first sub-subtracter, so that the bit width of the binary number obtained by shifting the difference value output by the first sub-subtracter is one bit lower than the bit width of the binary number of the accumulated sum value output by the first accumulator; and two input ends of the divider are respectively connected with the output end of the numerator adder and the output end of the denominator shift register, and the divider is used for controlling a dividend output by the numerator adder and a divisor output by the denominator shift register to perform division operation once after enabling, then sending the operated quotient value back to the interconnection bus, determining that the quotient value becomes a TSDF value corresponding to one pose in the search window, and also determining that the divider equivalently completes one weighted average processing of a truncation symbol distance function value of the currently stored point cloud mapping by executing one division operation.

The technical scheme is that weighted average processing of a truncation symbol distance function value is equivalent to integral operation taking a truncation distance as an integral variable and the number of currently stored point clouds as an integral upper limit, so that the technical scheme firstly combines the truncation symbol distance function value, a weight, a product of the truncation symbol distance function value and the weight on hardware, then carries out parallel accumulation processing on the same items, selects an accumulation item of the weight as a frequency division parameter of a divider, and then executes division operation on the basis of displacement processing of a dividend and a divisor; under the counting triggering action of the frequency division counter, the point cloud processing module is pushed to enter the TSDF value calculation module to execute a division operation under all the corresponding truncated symbol distance function values and weight values of the correlation scan matching algorithm in the current iteration state. Thereby saving the operation resource of the divider.

Further, the TSDF value calculating module comprises a grid index submodule, wherein the grid index submodule comprises a first index subtracter, a second index subtracter, a first index adder, a second index adder, a third index adder and an index multiplier; the first index subtracter and the second index subtracter belong to subtracters, and the first index adder and the second index adder belong to adders; the first input end of the first index subtracter is used for receiving the abscissa of the discrete point cloud transmitted by the interconnection bus, wherein the abscissa of the discrete point cloud transmitted by the interconnection bus to the first index subtracter is from the memory module; the second input end of the first index subtracter is used for receiving a horizontal axis coordinate offset value transmitted by the interconnection bus, wherein the preset coordinate offset value comprises 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 arranged in the correlation scanning matching hardware circuit and is used for storing a coordinate offset value associated with the pre-expanded grid map; the first input end of the first index adder is connected with the output end of the first index subtracter; the second input end of the first index adder is used for receiving the horizontal axis coordinate searching step length transmitted by the interconnection bus; the search step length comprises a horizontal axis coordinate search step length, the horizontal axis coordinate search step length transmitted by the interconnection bus is derived from the search step length stored in a search window parameter register, and the search window parameter register is a parameter register arranged in the correlation scan matching hardware circuit and used for storing the pose covered by the search window and associated search information; the sum value output by the first index adder is configured as a horizontal axis direction index value in a storage space for mapping the discrete point cloud to a truncated symbol distance function value; the first input end of the second index subtracter is used for receiving the vertical coordinate of the discrete point cloud transmitted by the interconnection bus, wherein the vertical coordinate of the discrete point cloud transmitted by the interconnection bus to the second index subtracter is from the memory module; a second input end of the second index subtracter is configured to receive a vertical axis coordinate offset value transmitted by the interconnection bus, where the preset coordinate offset value further 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 with the output end of the second index subtracter; the second input end of the second index adder is used for receiving the longitudinal axis coordinate searching step length transmitted by the interconnection bus; the search step length also comprises a longitudinal axis coordinate search step length, and the longitudinal axis coordinate search step length transmitted by the interconnection bus is also derived from the search step length stored in the search window parameter register; the sum value output by the second index adder is configured as a longitudinal axis direction index value in a storage space where the discrete point cloud maps to a truncated symbol distance function value.

The technical scheme uses two same addition and subtraction operation combined structures to respectively calculate a horizontal axis direction index value in a storage space mapped to the truncation symbol distance function value and a vertical axis direction index value in the storage space mapped to the truncation symbol distance function value, and realizes that the horizontal axis and the vertical axis of the discrete point cloud are converted into discrete index information in the storage space of the truncation symbol distance function value in parallel.

Further, the grid index sub-module further comprises a third index adder and an index multiplier, wherein 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 with the output end of the second index adder; a second input end of the index multiplier is used for receiving the row grid quantity transmitted by the interconnection bus, wherein the row grid quantity is the grid quantity which can be occupied by each row of the storage space of the truncated symbol distance function value, is stored in the map size register and is transmitted to the interconnection bus by the map size register; the map size register is a parameter register arranged in the correlation scanning matching hardware circuit and is used for storing the size range of the pre-expanded grid map and associated expanded information; the first input end of the third index adder is connected with the output end of the first index adder, the second input end of the third index adder is connected with the output end of the index multiplier, the third index adder is used for controlling the product of the row grid quantity and the sum value output by the second index adder to be added with the index value in the horizontal axis direction, the added sum value is set to be an index value of a storage space where the discrete point cloud is mapped to the truncated symbol distance function value, and the index value is sent to the interconnection bus, so that the truncated symbol distance function value and the weighted value matched with the index value can be read from the memory module.

In the technical scheme, an index multiplier controls the multiplication of the number of the line grids and the index value in the longitudinal axis direction, a third index adder controls the addition of the product output by the index multiplier and the index value in the transverse axis direction, and an index value in a storage space mapped to a truncated symbol distance function value by a discrete point cloud is calculated in a line scanning query mode and serves as an index value or an address of a linear storage space, so that the interconnection bus reads the truncated symbol distance function value and a weight value which are matched with the index value from the memory module.

Further, the grid index sub-module further comprises a third index adder and an index multiplier, wherein 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 with the output end of the first index adder; the second input end of the index multiplier is used for receiving the number of the column grids transmitted by the interconnection bus; the column grid number is the number of grids which can be occupied by each column of the storage space of the truncated symbol 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 arranged in the correlation scanning matching hardware circuit and is used for storing the size range of the pre-expanded grid map and associated expanded information; the first input end of the third index adder is connected with the output end of the second index adder, and the second input end of the third index adder is connected with the output end of the index multiplier; and the third index adder is used for controlling the addition of the product of the number of the column grids and the sum value output by the first index adder and the index value in the longitudinal axis direction, setting the sum value obtained by the addition as an index value in a storage space for mapping the discrete point cloud to the truncated symbol distance function value, and sending the index value to the interconnection bus so as to read out the truncated symbol distance function value and the weight value matched with the index value from the memory module.

In the technical scheme, an index multiplier controls the number of the columns of grids to be multiplied by the index value in the horizontal axis direction, a third index adder controls the product of the index value in the vertical axis direction and the product output by the index multiplier to be added, and an index value in a storage space mapped to a truncated symbol distance function value by a discrete point cloud is calculated in a column scanning query mode and serves as an index value or an address of a linear storage space, so that the interconnection bus can read the truncated symbol distance function value and a weight value which are matched with the index value from the memory module.

Further, the pre-expanded grid map is a grid map obtained by expanding transformation according to an expansion parameter required by a maximum detection radius reached by a currently stored point cloud, an expansion parameter required by executing coordinate system transformation, an expansion parameter required by executing rotation transformation and an expansion parameter required by aligning bus bit width on the basis of a map defined by an original point cloud; the original point clouds are all the laser point clouds obtained by scanning the laser radar for one circle currently; the map defined by the original point cloud is a grid map which is defined by taking a laser radar as a center and taking the maximum detection diameter reached in the original point cloud as the side length of the map; the alignment bus bit width refers to that the memory of the grid covered by the side length of the grid map expanded in advance is equal to the bit width of a bus for transmitting the grid map; the preset coordinate deviation value is determined by the sum of a coordinate translation parameter required by the maximum detection radius reached by the currently stored point cloud, a coordinate translation parameter required by executing the coordinate system transformation, a coordinate translation parameter required by executing the rotation transformation and a coordinate translation parameter required by aligning the bus bit width. The pre-expanded grid map is expanded according to the memory parameters occupied by the coordinate translation parameter, the coordinate rotation parameter and the map side length in sequence on the basis of the maximum coverage radius of the point cloud, so that the actual use size of the map can be constrained, the bus bit width used during map transmission is adapted, data alignment is ensured, and the storage space occupied by the TSDF map is reduced.

Further, the point cloud processing module comprises a point cloud discretization sub-module; the point cloud discretization sub-module comprises a first discrete adder, a first discrete subtracter, a first discrete multiplier, a second discrete adder, a second discrete subtracter and a second discrete multiplier, wherein the first discrete adder and the second discrete adder belong to the adders, the first discrete subtracter and the second discrete subtracter belong to the subtracters, and the first discrete multiplier and the second discrete multiplier belong to the multipliers; the first input end of the first discrete adder is used for receiving the abscissa of the 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; the abscissa of the point cloud belongs to the abscissa value under the laser coordinate system; the second input end of the first discrete adder is used for receiving the abscissa of the robot position transmitted by the interconnection bus, wherein the abscissa of the robot position is a pre-calculated abscissa value of the robot in a world coordinate system; the first discrete adder is used for controlling the addition of the abscissa of the point cloud and the abscissa of the robot position and outputting a sum obtained by the addition so that the sum becomes a horizontal axis coordinate value of the point cloud converted into a world coordinate system; the first input end of the first discrete subtracter is connected with the output end of the first discrete adder; the second input end of the first discrete subtracter is used for receiving the maximum abscissa value of the map transmitted by the interconnection bus, and the maximum abscissa value of the map is derived from the map size register and is transmitted to the interconnection bus by the map size register; the map size register is a parameter register arranged in the correlation scanning matching hardware circuit and used for storing the size range of the grid map meeting the transmission requirement of the bus bit width; the difference value output by the first discrete subtracter becomes the longitudinal coordinate value of the grid map which meets the transmission requirement of the bus bit width; the first input end of the first discrete multiplier is connected with the output end of the first discrete subtracter; the second input end of the first discrete multiplier is used for receiving the reciprocal of the map resolution transmitted by the interconnection bus, and the reciprocal of the map resolution is derived from a map resolution register and is transmitted to the interconnection bus by the map resolution register; the map resolution register is a parameter register arranged in the correlation scanning matching hardware circuit and used for storing the resolution information of the raster map meeting the transmission requirement of the bus bit width; the product value output by the first discrete multiplier is configured to be the abscissa value of the discrete point cloud so as to complete coordinate system transformation, and the aim of aligning the abscissa value of the currently stored point cloud into the coordinate system of the grid map meeting the transmission requirement of the bus bit width is achieved; the first discrete multiplier is also used for outputting a product value obtained by multiplying to the memory module through the interconnection bus so as to prepare for the TSDF value calculation module to read the abscissa value of the discrete point cloud; the first input end of the second discrete adder is used for receiving the vertical coordinate of the point cloud transmitted by the interconnection bus, wherein the point cloud transmitted by the interconnection bus to the second discrete adder is from the currently stored point cloud stored in the memory module; the vertical coordinate of the point cloud belongs to the vertical coordinate value under the laser coordinate system; the second input end of the second discrete adder is used for receiving 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 coordinate value of the robot in a world coordinate system; the second discrete adder is used for controlling the addition of the vertical coordinate of the point cloud and the vertical coordinate of the position of the robot and outputting a sum value obtained by the addition so that the sum value becomes a vertical axis coordinate value of the point cloud converted into a world coordinate system; the first input end of the second discrete subtracter is connected with the output poison case of the second discrete adder; a second input end of the second discrete subtractor, configured to receive a map maximum ordinate value transmitted by the interconnection bus, where the map maximum ordinate value is derived from the map size register and is transmitted to the interconnection bus by the map size register; the difference value output by the second discrete subtracter becomes the abscissa value of the grid map which meets the transmission requirement of the bus bit width; the first input end of the second discrete multiplier is connected with the output end of the second discrete subtracter; the second input end of the second discrete multiplier is used for receiving the reciprocal of the map resolution transmitted by the interconnection bus; the product value output by the second discrete multiplier is configured to be a longitudinal coordinate value of the discrete point cloud so as to complete coordinate system transformation and align the longitudinal coordinate value of the currently stored point cloud into a coordinate system of a grid map meeting the transmission requirement of bus bit width; and the second discrete multiplier is also used for outputting a product value obtained by multiplying to the memory module through the interconnection bus so as to prepare for the TSDF value calculation module to read the longitudinal coordinate value of the discrete point cloud.

For each point cloud, reading an abscissa and an ordinate from a block storage space in which the point cloud is stored, inputting the point cloud discretization sub-module, and transforming the abscissa and the ordinate of the point cloud from a point cloud coordinate system to a coordinate system of the pre-expanded grid map in the same offset mode along the respective adaptive coordinate axis direction through an adder and a subtracter, wherein the coordinate system transformation is realized by exchanging the coordinate axis of the point cloud; and then multiplied by the inverse of the grid map resolution to complete the discretization process of the point cloud.

Further, the point cloud processing module further comprises a point cloud rotation sub-module; the point cloud rotation sub-module comprises 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 subtracter, wherein the first point cloud multiplier, the second point cloud multiplier, the third point cloud multiplier and the fourth point cloud multiplier belong to multipliers, the point cloud adder belongs to the adder, and the point cloud subtracter belongs to the subtracter; the first input end of the first point cloud multiplier is connected with the output end of the first register; the point cloud discretization submodule is internally provided with a first register; the abscissa of the point cloud transmitted to the first input end of the first discrete adder by the interconnection bus is cached in a first register; the second input end of the first point cloud multiplier is used for receiving a cosine function value corresponding to a rotation angle reached by the current rotation transformation, wherein the rotation angle reached by the current rotation transformation is the sum of an angle search step length and the rotation angle reached by the last rotation transformation; the angle searching step length is stored in a searching window parameter register; the search window parameter register is a parameter register arranged in the correlation scanning matching hardware circuit and used for storing the pose covered by the search window and the associated stepping search information; the first input end of the second point cloud multiplier is connected with the output end of the first register; 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 in a second register; the second input end of the second point cloud multiplier is used for receiving a sine function value corresponding to the rotation angle reached by the current rotation transformation; a first input end of the third point cloud multiplier is connected with an output end of the first register; a second input end of the third point cloud multiplier, configured to receive a sine function value corresponding to the rotation angle reached by the current rotation transformation; the first input end of the fourth point cloud multiplier is connected with the output end of the second register; a second input end of the fourth point cloud multiplier, configured to receive a cosine function value corresponding to the rotation angle reached by the current rotation transformation; the first input end of the point cloud subtracter is connected with the output end of the first point cloud multiplier, the second input end of the point cloud subtracter is connected with the output end of the second point cloud multiplier, and the difference value output by the point cloud subtracter is set as the abscissa value of the rotating point cloud obtained after the point cloud is subjected to current rotation transformation; the point cloud subtracter is also used for transmitting the output difference value to the memory module through the interconnection bus and covering the abscissa of the point cloud stored in the memory module so as to update the abscissa result converted from the current rotation to the abscissa of the currently stored point cloud stored in the memory module; the first input end of the point cloud adder is connected with the output end of the third point cloud multiplier, the second input end of the point cloud adder is connected with the output end of the fourth point cloud multiplier, and the sum value output by the point cloud adder is set as the longitudinal coordinate value of the rotating point cloud obtained after the point cloud is subjected to current rotation transformation; the point cloud adder is also used for transmitting the output difference value to the memory module through the interconnection bus and covering the vertical coordinate of the point cloud stored in the memory module so as to update the vertical coordinate result converted from the current rotation to the vertical coordinate of the currently stored point cloud stored in the memory module; the rotation times are determined by a preset angle searching range of the searching window and a preset angle searching step length; each rotation transformation corresponds to one coordinate system transformation executed by the point cloud processing module and belongs to one matching processing of correlation scanning matching of the map.

The technical scheme is that a rotation matrix is used as a basic operation framework, a point cloud rotation submodule is constructed by only using an adder, a subtracter and a multiplier and is used for receiving and processing a trigonometric function value corresponding to a stepping rotation angle calculated in advance by software, a trigonometric function hardware calculation module is omitted, and the point cloud rotation submodule is controlled to carry out iterative rotation transformation on the basis of stepping rotation to obtain the point cloud which is used for updating the original memory in the memory module. According to the technical scheme, the traversal of each pose of the search window is completed by combining the point cloud discretization sub-module, so that the TSDF value calculating module is promoted to construct a weighting function of each pose of the search window.

Furthermore, the state machine control module belongs to a finite state machine; the state machine control module is used for scheduling the working states of the memory module, the point cloud processing module and the TSDF value calculating module, so that when the point cloud processing module executes the coordinate system transformation and the rotation transformation once, the TSDF value calculating module executes the division operation once to obtain a TSDF value matched with a pose corresponding to the searching in the searching window, and on the basis of updating the currently stored point cloud by using the result obtained by the rotation transformation, the point cloud processing module is controlled to continuously execute the coordinate system transformation and the TSDF value calculating module is controlled to execute the division operation once again until the TSDF value calculating module counts all poses in the searching window and calculates all TSDF values matched with poses in sequence. The searching step length is the number of grid searching which needs to be traversed by searching for one pose in the same searching window; the search window is a neighborhood grid region centered around a point cloud within the pre-expanded grid map. In this technical solution, the state machine control module finishes the correlation scan matching corresponding to the pose in the search window in a way of hardware scheduling of a working state signal, and includes that on the premise that the TSDF value calculation module calculates the TSDF value of one pose in the search window every time, the point cloud processing module is controlled to sequentially perform the rotation and translation transformation on each point cloud, so as to read out a truncated symbol distance function value and a weight value matched with an index value from the memory module, thereby implementing the operation in a cyclic state, ensuring the hardware normal operation of correlation scan matching, and accelerating the convergence speed of the algorithm.

Furthermore, a bus interface module is arranged outside the correlation scanning matching hardware circuit, and comprises a DMA (direct memory access) controller module and a transmission bus; the DMA controller module is used for continuously transmitting data stored in the physical storage space of the discontinuous addresses in batches and reducing the triggering times of software interruption of the CPU; the transmission bus comprises a first bus and a second bus, the first bus is respectively in signal transceiving connection 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, the first bus is used for configuring data transmission parameters for the DMA controller module, and the first bus is also used for configuring parameters stored in the map size register, parameters stored in the map resolution register, expansion parameters required by the maximum detection radius reached by the currently stored point cloud, expansion parameters required by executing the coordinate system transformation, expansion parameters required by executing the rotation transformation, expansion parameters required by meeting the transmission requirement of the bus bit width and search step length so as to realize mapping communication of the memory of the correlation scanning matching hardware circuit; the second bus is connected with the DMA controller module and is used for transmitting a truncated symbol distance function configured in advance by the CPU and a weight value matched with the truncated symbol distance function to the memory module; wherein the transmission bus is in compliance with AMBA protocol.

The technical scheme provides a bus interface architecture module for the correlation scanning matching hardware circuit and an external data source (controller), and respectively designs a first bus suitable for simple and low-throughput memory mapping communication (transmitting extension parameters, map characteristic parameters and associated basic control signals to a register inside the correlation scanning matching hardware circuit) and a second bus for high-speed data flow (transmitting a truncated symbol distance function value and a weighted value to the memory module) according to data transmission performance; the real-time performance of the operation work of the correlation scanning matching hardware circuit is improved.

A heterogeneous computing architecture comprises a processor unit, a bus interface and a correlation scanning matching hardware circuit, wherein the processor unit and the correlation scanning matching hardware circuit are interconnected through the bus interface, the processor unit does not control parallel acceleration operation executed by the correlation scanning matching hardware circuit, and the processor unit only provides operation parameters, a starting signal, an interrupt signal, a reset signal and a zone bit clearing signal for the correlation scanning 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 circuitry; 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.

Drawings

Fig. 1 is a schematic diagram of a TSDF map-based correlation scan matching hardware circuit according to an embodiment of the present invention.

Fig. 2 is a schematic diagram of a TSDF calculating module according to another embodiment of the present invention.

FIG. 3 is a schematic diagram of a point cloud processing module according to another embodiment of the present invention.

Detailed Description

The following description of the embodiments of the present invention will be made with reference to the accompanying drawings. It should be noted that the Memory module in the above embodiments is essentially a storage medium, and the storage medium may be, but is not limited to, a Read-Only Memory (ROM), a Random Access Memory (RAM), and other storage media capable of storing program codes; the point cloud processing module, the TSDF value calculating module, the state machine control module, the interconnection bus, each calculating unit in the point cloud processing module and each calculating unit in the TSDF value calculating module disclosed in the embodiments of the present invention in the TSDF map correlation scan matching hardware circuit can be, but not limited to, a digital circuit module compiled by a designer using a hardware description language Verilog HDL, or a digital circuit module compiled by a designer on software having a circuit drawing or compiling function. In addition, functional units in the embodiments of the present invention may be integrated into one processing module, or each unit may exist alone physically, or two or more units are integrated into one module.

The embodiment of the invention discloses a correlation scan matching hardware circuit based on a TSDF (time series derived) map, which is electrically connected with a laser radar, is used for calculating a TSDF value of a point cloud aligned to a grid map and mapped to a truncated symbol distance field, and is used for evaluating the positioning accuracy of a pose of a currently acquired laser point cloud in a search window of the grid map, wherein the TSDF has better scene reconstruction accuracy, and particularly in the field of mobile robot SLAMs (SLAMs), the TSDF map is more suitable for an optimization-based SLAM algorithm relative to an occupied grid map. The laser radar can be arranged on the mobile robot, and a laser probe of the laser radar keeps collecting laser point cloud frame data in the rotating process and records the laser point cloud frame data as point cloud; the scanning range of the laser radar can take the center of the robot body as a scanning center, and can also take the center of the laser radar as the scanning center; in the implementation, each time the laser radar rotates for one circle, the point cloud acquired by the circle is controlled to be sent to the correlation scanning matching hardware circuit so as to calculate the TSDF value once and match a pose in the search window. The world coordinate system has a specific coordinate transformation relation relative to the two-dimensional grid map coordinate system, and the position of the body center of the robot in the two-dimensional grid map coordinate system is calculated according to the specific coordinate transformation relation; then, according to the currently acquired point cloud data, the maximum range reached by the point cloud is counted.

The correlation scanning matching hardware circuit comprises a memory module, a point cloud processing module, a TSDF value calculating module, a state machine control module and an interconnection bus, wherein the memory module, the point cloud processing module, the TSDF value calculating module and the state machine control module all perform data transmission through the interconnection bus, the interconnection bus is used as an interconnection structure of the correlation scanning matching hardware circuit and plays roles of data transmission and data sharing among all circuit modules in the correlation scanning matching hardware circuit, so that the point cloud processing module, the TSDF value calculating module and the state machine control module are all connected to a memory space of the memory module through the interconnection structure, and the interconnection bus performs communication and data transmission with an external processor or an external bus through a bus interface arranged in the correlation scanning matching hardware circuit; the state machine control module is used as a finite state machine and is configured as a control unit in the correlation scanning matching hardware circuit, and the control unit is used for scheduling data transmission of the interconnection bus. If the correlation scanning matching hardware circuit is integrated in a chip, the interconnection bus belongs to an on-chip interconnection bus and is responsible for on-chip data communication, but the interconnection bus can be communicated with an off-chip processor or other off-chip equipment through a bus interface arranged on the chip.

And the Point Cloud processing module is used for reading the currently stored Point Cloud from the memory module under the control of the state machine control module, specifically, reading the existing Point Cloud from the box marked with the Point Cloud shown in fig. 1, and then controlling the read Point Cloud to execute coordinate system transformation, including offset transformation of horizontal and vertical coordinate values and proportion adjustment of map resolution, so as to realize coordinate transformation and discretization processing, and finally obtaining a discretization result as a coordinate system transformation result defined by the embodiment. Setting the result of the coordinate system transformation as discrete point cloud, specifically falling into the coordinate system of the grid map expanded in advance, namely aligning the currently stored point cloud into the coordinate system of the grid map expanded in advance; then storing the result of the coordinate system transformation into a memory module, specifically storing the set Discrete Point Cloud into a box marked as Discrete Point Cloud in fig. 1; the currently stored Point Cloud, namely the Point Cloud existing in the square frame marked with the Point Cloud, is acquired from the laser radar, and the updating and covering of the Point Cloud subjected to subsequent transformation are supported, so that the currently stored Point Cloud is not necessarily acquired as the original Point Cloud. It should be noted that, in the foregoing grid map, the value in each grid represents the probability that the grid is occupied; the point cloud and the pre-expanded grid map are aligned to be understood as follows: and (3) aligning the point cloud for representing the obstacle scanned by the laser radar with the obstacle grid in the pre-expanded grid map, preferably, calling an associated computing unit by the point cloud processing unit to rotate and translate so as to realize the coincidence of the point cloud and the obstacle.

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, read an existing Point Cloud from a box marked with a Point Cloud shown in fig. 1, and then control the read Point Cloud to perform rotation transformation so as to enable the Point Cloud to complete coverage of an angle search range of a search window according to a preset angle search step length, preferably, the embodiment optimizes the rotation transformation of the currently read Point Cloud into step rotation, and ensures that an angle of each rotation is obtained by rotating the preset angle search step length once again on the basis of an angle obtained by the previous rotation, so that the rotation transformation does not start to rotate from an initial position every time; then, updating the result of the rotation transformation to the currently stored Point Cloud stored in the memory module, namely updating the Point Cloud stored in the block which is marked with Point Cloud, so that the updated Point Cloud (the currently stored Point Cloud) is configured to execute the coordinate system transformation and determine to complete the primary matching processing of the correlation scanning matching of the map; it is emphasized that each matching process of the correlation scan matching of the map comprises a coordinate system transformation, and the transformation is performed based on the pre-expanded grid map.

A TSDF value calculating module, which is used for reading the Discrete Point Cloud from the memory module under the control of the state machine control module, reading the Discrete Point Cloud in the frame marked as Discrete Point Cloud in figure 1, then calculating the index value of the Discrete Point Cloud mapped to the storage space of the truncated symbol distance function value according to the preset coordinate deviant and the search step length, matching the rotation and translation operation of the pre-expanded grid Map, and setting the index value as the reading address of the truncated symbol distance function value and the reading address of the weight value stored in the memory module, specifically the Map marked as Map of TSDF in figure 1 _i (x) Has a truncated symbol distance function value tsdf in the frame of (a) _i (x) Read address of (1), labeled Map of weight in FIG. 1 _i (x) Has weight value weight in the frame _i (x) Wherein, i represents the number of point clouds, which is the number of point clouds obtained by scanning a circle by the laser radar; the truncation symbol distance function value and the weight value both have a matched index value; the searching step length is the number of grid searching which needs to be traversed when one pose is searched each time in the same searching window; it should be noted that the search window is an adjacent window in the pre-expanded grid map, centered on a point cloudA domain grid area, where there is one search window for each point cloud.

The TSDF value calculating module is also used for reading a truncated symbol distance function value TSDF from the memory module according to the currently set reading address under the control of the state machine control module _i (x) And weight value weight matched with the weight value _i (x) Wherein the currently read truncated sign distance function value tsdf _i (x) Is equal to the weight value weight currently read out _i (x) The number of (2); then controlling a divider built in a TSDF value calculation module to equivalently complete the weighted average processing of the truncated symbol distance function value of the currently stored point cloud mapping by executing a division operation, wherein before executing the division operation, the method also comprises the steps of accumulating and summing the read data to obtain a dividend item and a divisor item; then setting the result output by each operation of the divider as a TSDF value, compared with the prior art, the embodiment adopts a hardware division operation to obtain the TSDF value for the positioning precision of a corresponding pose searched in the representation search window; in this embodiment, each time the point cloud processing module completes one matching process (including coordinate system transformation and rotation transformation executed successively) of the correlation scan matching of the map, the divider built in the TSDF value calculation module executes one division operation to obtain a TSDF value corresponding to a new pose in the search window, which is used as a result of the correlation scan matching hardware circuit executing one acceleration operation. Preferably, the truncated symbol distance function value tsdf _i (x) With a weight value weight to evaluate its uncertainty _i (x) Is in direct proportion.

The memory module is used for storing the currently stored point cloud, the discrete point cloud obtained by the currently stored point cloud through the conversion of the point cloud processing module, the preset truncated symbol distance function and the matched weight value thereof; each type of data is stored in different Block memory spaces of the same memory module, specifically, as shown in fig. 1, the memory module is divided into the aforementioned Block marked with Point Cloud (which may belong to a Block RAM) and the aforementioned Block marked with secret Point Cloud according to the memory content(may belong to Block RAM), the aforementioned tag is Map of tsdf _i (x) Within the box (which may belong to Block RAM), the foregoing is marked with Map of weight _i (x) May belong to Block RAM.

Therefore, in this embodiment, for each pose in the search window in the grid map expanded in advance, the TSDF value calculation module is invoked to perform a division operation to obtain the TSDF value corresponding to the pose, and the division operation is scheduled and performed by the state machine control module on the basis of the discrete point cloud provided by the point cloud processing module and the result of the rotational transformation for updating the discrete point cloud provided by the point cloud processing module, and it is ensured that the associated operation result and the transformation result are shared on the interconnection bus; compared with the prior art, the hardware operation mode of the technical scheme reduces the division operation amount in hardware, does not increase the additional storage space of the hardware, and reduces the consumption of a large amount of programmable logic resources.

As an embodiment, as shown in fig. 2, the TSDF value calculating module includes a selector, a division counter, a pre-calculation unit, a divider, a first accumulator, a second accumulator, and a third accumulator; the first accumulator is labeled SUM tsdf in FIG. 2 _i (x) For controlling said truncated symbol distance function value tsdf _i (x) Performing an accumulated summation, wherein the number of accumulated terms is i, i being equal to the number of currently stored point clouds. The second accumulator is labeled SUM weight in FIG. 2 _i (x) For controlling said weight values weight _i (x) Performing an accumulated summation, wherein the number of accumulated terms is i, i being equal to the number of currently stored point clouds. The third accumulator is labeled SUM tsdf in FIG. 2 _i (x)*weight _i (x) And the block is used for receiving the truncated symbol distance function value and the weighted value belonging to the same value i, and then controlling the product of the truncated symbol distance function value and the weighted value belonging to the same value i to perform accumulation summation, wherein the accumulated number of terms is i, and i is equal to the number of the currently stored point clouds. The divide Counter is the box labeled Division Counter in FIG. 2; the TSDF value calculation module is used for cutting the data under the control of the state machine control moduleThe function value of the truncated symbol distance is sent to a first accumulator, the weight value is sent to a second accumulator, the product of the function value of the truncated symbol distance and the matched weight value is sent to a third accumulator, and each accumulator is controlled to simultaneously execute accumulation operation; wherein the input ends of the three accumulators are configured to receive the data transmitted by the interconnection bus in sequence; the output ends of the three accumulators are respectively connected with the corresponding data input ends of the selector, the selection input end of the selector is connected with the counting output end of the frequency division counter, the selector is provided with a data output end matched with each data input end and used for gating and outputting data received by the data input end of the selector under the counting triggering action of the frequency division counter so as to adapt to the enabling time sequence of the divider, the embodiment simultaneously gates and outputs the accumulation result output by the first accumulator, the accumulation result output by the second accumulator and the accumulation result output by the third accumulator, and each data output end of the selector is respectively connected with the corresponding input end of the pre-calculation unit and used for transmitting the accumulation sum of the products of the truncation symbol distance function values and the weight values, the accumulation sum of the truncation symbol distance function values and the accumulation sum of the weight values to the pre-calculation unit.

Specifically, the pre-calculation unit comprises a numerator adder, a first numerator subtracter, a second numerator subtracter, a numerator shift register, a first numerator subtracter and a denominator shift register; the numerator adder belongs to the adder, the first numerator subtracter and the second numerator subtracter both belong to subtractors, and the numerator shift register and the denominator shift register both belong to shift registers; the first input end of the second sub-subtracter is connected with the output end of the first accumulator, and the second input end of the second sub-subtracter is connected with the output end of the third accumulator; and the second sub-subtracter is used for controlling the accumulated sum value output by the first accumulator to be subtracted from the accumulated sum value output by the third accumulator and outputting the difference value of the two subtracted data.

The first molecular subtracter is provided with an input end for receiving the quantity value of the currently stored point cloud transmitted by the interconnection bus; the other input end of the first molecular subtracter is connected with the output end of the second accumulator; the first sub-subtracter is used for controlling 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 outputting the difference value of two subtracted data, specifically the result of the connection of two input ends of the first sub-subtracter and the corresponding data output end of the selector; the input end of the molecular shift register is connected with the output end of the first molecular subtracter, and the molecular shift register is used for shifting the difference value output by the first molecular subtracter, so that the bit width of a binary number obtained by shifting the difference value output by the first molecular subtracter is equal to the bit width of an accumulated sum value output by the first accumulator, and preferably, the shift digit number executed by the molecular shift register is equal to the difference value between the bit width of the binary number equivalent to the truncation symbol distance function value and the bit width of the binary number equivalent to the weight value. And the numerator adder is used for adding the shift output result of the numerator shift register with the difference value output by the second numerator subtracter and outputting the sum value of the added two data to the corresponding input end of the divider so that the sum value output currently is configured as a dividend.

The first sub-subtracter is used for controlling 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 outputting the difference value of two subtracted data, specifically the result of the connection of two input ends of the first sub-subtracter and the corresponding data output end of the selector; the input end of the denominator shift register is connected with the output end of the first subtracter, the denominator shift register is used for shifting the difference value output by the first subtracter, and then the data obtained through shifting is configured to be divisor to be output to the corresponding input end of the divider, so that the bit width of the binary number obtained through shifting the difference value output by the first subtracter is lower than the bit width of the binary number of the accumulated sum value output by the first accumulator by one bit, and the divisor output by the first subtracter is configured to be the frequency division coefficient of the divider.

Two input ends of a divider are respectively connected with an output end of a numerator adder and an output end of a denominator shift register, the divider is used for enabling the divider when a divisor and a dividend calculated by a hardware unit are assigned to an input end of the divider after being enabled, dozens of levels of running water are arranged in the divider, the divider outputs enabling after a plurality of clock cycles under the counting triggering action of a frequency dividing counter, the dividend output by the numerator adder and the divisor output by the denominator shift register are controlled to perform division operation once, the operated quotient value is sent back to the interconnection bus, the quotient value is determined to be a TSDF value corresponding to a pose in the search window, and the divider is also determined to equivalently complete a weighted average processing of symbol distance function values intercepted by mapping of the currently stored point cloud or a weight increment updating mode in a software algorithm by executing the division operation once.

Compared with the prior art, the TSDF value calculating module is equivalent to integral operation which takes the truncation distance as an integral variable and the number of currently stored point clouds as an integral upper limit, so that the technical scheme firstly combines the truncation symbol distance function value, the weight and the product of the truncation symbol distance function value and the weight on hardware to obtain the same type (accumulation and value item), then performs parallel accumulation on the same type, selects the accumulation item of the weight as a frequency division parameter of a divider, and then performs division operation on the basis of shifting the dividend and the divisor; under the counting triggering action of the frequency division counter, the point cloud processing module is pushed to enter the TSDF value calculation module to execute a division operation under all the corresponding truncated symbol distance function values and weight values of the correlation scan matching algorithm in the current iteration state. Thereby saving the operation resource of the divider.

As an embodiment, as can be seen in fig. 1 and 2, the TSDF value calculating module includes a grid indexing submodule including a first index subtractor, a second index subtractor, a first index adder, a second index adder, a third index adder, and an index multiplier; the first index subtracter and the second index subtracter belong to subtracters, and a circle marked with "-" is arranged in a grid index submodule of FIG. 2; the first index adder, the second index adder and the third index adder all belong to adders, and are circles marked with "+" in the grid index submodule of fig. 2; the index multiplier belongs to a multiplier, and is a circle marked with an "x" in the grid index submodule of fig. 2.

The first input end of the first index subtracter is used for receiving the abscissa of the discrete point cloud transmitted by the interconnection bus, preferably, the abscissa of the discrete point cloud transmitted by the interconnection bus is supported to be registered in a register inside the grid index submodule, and the first input end of the first index subtracter receives the abscissa of the discrete point cloud transmitted by the interconnection bus through the connected register. In the embodiment shown in FIG. 2, the first input terminal of the first index subtracter (the first subtracter arranged from top to bottom in the grid index submodule of FIG. 2) and the register DP _x (marked with DP _x Block of) in which the register DP is connected _x The system comprises a first index subtracter, a second index subtracter, a first bus and a second bus, wherein the first index subtracter is used for buffering the abscissa of the discrete point cloud transmitted by the interconnection bus, and then transmitting the abscissa of the currently buffered discrete point cloud to the first input end of the first index subtracter to enable the first input end of the first index subtracter to receive the abscissa of the discrete point cloud transmitted by the interconnection bus; wherein the discrete point cloud transmitted by the interconnection bus to the first index subtractor originates from the memory module. The second input end of the first index subtracter is configured to receive the abscissa offset value transmitted by the interconnection bus, and preferably, the abscissa offset value transmitted by the interconnection bus is supported to be registered in a register inside the grid index submodule, and the second input end of the first index subtracter receives the abscissa offset value transmitted by the interconnection bus through a connected register. In the embodiment shown in FIG. 2, the second input of the first index subtractor and the register Offset _x (marked with Offset) _x Block of) is connected, wherein register Offset _x Used for buffering the horizontal axis coordinate deviation value transmitted by the interconnection bus and then shifting the current buffered horizontal axis coordinateAnd the value is transmitted to a second input end of the first index subtracter, so that the second input end of the first index subtracter receives the horizontal axis coordinate offset value transmitted by the interconnection bus. The preset coordinate deviation value comprises a horizontal axis coordinate deviation value, and the horizontal axis coordinate deviation value transmitted by the interconnection bus is derived from the preset coordinate deviation value stored in the map deviation value register; the map offset value register is a parameter register arranged in the correlation scanning matching hardware circuit and is used for storing a coordinate offset value associated with the pre-expanded grid map; the parameter register set shown in fig. 1 includes a map offset value register. And the first index subtracter is used for controlling the subtraction of the abscissa of the discrete point cloud received by the first input end and the abscissa offset value received by the second input end and outputting a difference value.

In this embodiment, the first input end of the first index adder is connected to the output end of the first index subtracter to construct a combinational logic circuit; the second input end of the first index adder is used for receiving the horizontal axis coordinate searching step length transmitted by the interconnection bus; preferably, the horizontal axis coordinate search step transmitted by the interconnection bus is supported to be registered 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 by the interconnection bus through a connected register. In the embodiment shown in FIG. 2, the second input terminal of the first index adder is connected to the register Step _x (labeled with Step) _x Block of (1), wherein register Step _x And the searching step length of the horizontal axis coordinate transmitted by the interconnection bus is cached, and then the cached searching step length of the horizontal axis coordinate is transmitted to the second input end of the first index adder. The searching step length comprises a horizontal axis coordinate searching step length, the horizontal axis coordinate searching step length transmitted by the interconnection bus is derived from the searching step length stored in a searching window parameter register, and the searching step length comprises the horizontal axis coordinate searching step length; the search window parameter register is the correlation scan matching hardware circuitThe parameter register is arranged inside and used for storing the pose covered by the search window and the associated search information; the parameter register set shown in FIG. 1 including a search window parameter register. And the first index adder is used for controlling the addition of the horizontal axis coordinate search step length and the difference value output by the first index subtracter, outputting the sum obtained by the addition, and determining the sum as a horizontal axis direction index value in a storage space where the discrete point cloud is mapped to the truncation symbol distance function value.

The first input end of the second index subtracter is used for receiving the vertical coordinate of the discrete point cloud transmitted by the interconnection bus, preferably, the vertical coordinate of the discrete point cloud transmitted by the interconnection bus is supported to be registered in a register inside the grid index submodule, and the first input end of the second index subtracter receives the vertical coordinate of the discrete point cloud transmitted by the interconnection bus through the connected register. In the embodiment shown in FIG. 2, a first input terminal of the second index subtracter (the second subtracter arranged from top to bottom in the grid index submodule of FIG. 2) and a register DP _y (marked with DP _y Block) of the register DP, wherein the register DP is connected to the output of the register DP _y The device is used for caching the vertical coordinate of the discrete point cloud transmitted by the interconnection bus, transmitting the vertical coordinate of the currently cached discrete point cloud to the first input end of the second index subtracter, and enabling the first input end of the second index subtracter to finish receiving the vertical coordinate of the discrete point cloud transmitted by the interconnection bus; wherein the discrete point cloud transmitted by the interconnection bus to the second index subtractor is derived from the memory module.

The second input end of the second index subtracter is configured to receive the vertical axis coordinate offset value transmitted by the interconnection bus, and preferably, the vertical axis coordinate offset value transmitted by the interconnection bus is supported to be registered in a register inside the grid index submodule, and the second input end of the second index subtracter receives the vertical axis coordinate offset value transmitted by the interconnection bus through a connected register. In the embodiment shown in FIG. 2, the second input of the second index subtractor and the register Offset _y (marked with Offset) _y Square frame of) are connectedThen, among them, register Offset _y The vertical axis coordinate deviation value is used for buffering the vertical axis coordinate deviation value transmitted by the interconnection bus, and then the current buffered vertical axis coordinate deviation value is transmitted to a second input end of a second index subtracter; the preset coordinate deviation value also comprises a vertical axis coordinate deviation value, and the vertical axis coordinate deviation value transmitted by the interconnection bus is also derived from the preset coordinate deviation value stored in the map deviation value register. And the second index subtracter is used for controlling the subtraction of the ordinate of the discrete point cloud received by the first input end and the ordinate offset value received by the second input end and outputting a difference value.

The first input end of the second index adder is used for receiving the difference value output by the second index subtractor, and in this embodiment, the first input end of the second index adder is connected with the output end of the second index subtractor to construct a combinational logic circuit; the second input end of the second index adder is configured to receive the vertical axis coordinate search step length transmitted by the interconnection bus, and preferably, the vertical axis coordinate search step length transmitted by the interconnection bus is supported to be registered 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 length transmitted by the interconnection bus through a register connected to the second input end of the second index adder. In the embodiment shown in FIG. 2, the second input of the second index adder is coupled to the register Step _y (labeled with Step) _y Block of (1), wherein register Step _y And the searching step length of the longitudinal axis coordinate transmitted by the interconnection bus is cached, and then the searching step length of the current cached longitudinal axis coordinate is transmitted to the second input end of the second index adder. The search step length also comprises a longitudinal axis coordinate search step length, and the longitudinal axis coordinate search step length transmitted by the interconnection bus is also derived from the search step length stored in the search window parameter register; the search step comprises a vertical axis coordinate search step. And the second index adder is used for controlling the searching step length of the longitudinal axis coordinate to be added with the difference value output by the second index subtracter, outputting the sum value obtained by addition and determining the sum value as the longitudinal axis direction index value in the storage space of the discrete point cloud mapped to the truncation symbol distance function value.

The foregoing embodiment uses two identical add-subtract operation combination structures to respectively calculate the horizontal axis direction index value in the storage space mapped to the truncated symbol distance function value and the vertical axis direction index value in the storage space mapped to the truncated symbol distance function value, so as to implement parallel conversion of the horizontal and vertical coordinates of the discrete point cloud into the discrete index information in the storage space of the truncated symbol distance function value.

As a preferred example, as can be seen from fig. 1 and 2, the grid index sub-module further includes a third index adder and an index multiplier; the third index adder belongs to an adder, and corresponds to the first adder arranged from left to right in the grid index submodule of FIG. 2; the index multipliers belong to the multipliers, belonging to the circles marked with an "x" uniquely in the grid index sub-module of fig. 2.

In this embodiment, the first input end of the index multiplier is connected to the output end of the second index adder to construct a combinational logic circuit; the second input end of the index multiplier is used for receiving the row grid number transmitted by the interconnection bus, preferably, the row grid number transmitted by the interconnection bus is registered in a register inside the grid index submodule, and the second input end of the index multiplier receives the row grid number transmitted by the interconnection bus through a connected register. In the embodiment shown in FIG. 2, the second input of the index multiplier is coupled to a register NUM _x (marked with NUM) _x Block of) phase, wherein the register NUM _x The second input end of the index multiplier is used for receiving the row grid quantity transmitted by the interconnection bus; wherein the number of line grids is the number of grids that can be occupied by each line of the storage space of the truncated symbol distance function value, and is stored in a map size register and transmitted to the interconnection bus by the map size register; parameters shown in FIG. 1The register set includes the map size register. The map size register is a parameter register arranged in the correlation scan matching hardware circuit and is used for storing the size range of the pre-expanded grid map and associated expanded information. And the index multiplier is used for controlling the multiplication of the number of the row grids and the sum value output by the second index adder and then outputting the product.

The first input end of the third index adder is configured to receive the index value in the horizontal axis direction output by the first index adder, and the second input end of the third index adder is configured to receive the product output by the index multiplier; and the third index adder is used for controlling the addition of the product of the row grid number and the sum value output by the second index adder and the index value in the direction of the transverse axis, setting the sum value obtained by the addition as an index value in a storage space for mapping the discrete point cloud to the truncated symbol distance function value, and sending the index value to the interconnection bus so as to read out the truncated symbol distance function value and the weight value matched with the index value from the memory module. Therefore, in this preferred embodiment, the index multiplier controls the number of line grids to be multiplied by the index value in the longitudinal axis direction, and then the third index adder controls the product of the index value in the transverse axis direction and the output of the index multiplier to be added, and the index value in the storage space where the discrete point cloud is mapped to the truncated symbol distance function value is calculated in a line scanning query manner, and is used as the index value or address of the linear storage space, so that the interconnection bus can read out the truncated symbol distance function value and the weight value which match the index value from the memory module. The method is suitable for a data parallel algorithm, and promotes real-time processing of a truncation symbol distance function value and a weight value of the truncation symbol distance function value of a high frame rate.

As another preferred example, the grid index sub-module 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 the preferred embodiment is not shown in the drawings of the specification. The first input end of the index multiplier is used for receiving the sum value output by the first index adder; the second input end of the index multiplier is used for receiving the number of the column grids transmitted by the interconnection bus; the column grid number is the number of grids which can be occupied by each column of the storage space of the truncated symbol 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 arranged in the correlation scanning matching hardware circuit and used for storing the size range of the grid map expanded in advance and associated expanded information; the index multiplier is used for controlling the number of the column grids to be multiplied by the sum value output by the first index adder and then outputting the product; the first input end of the third index adder is used for receiving the longitudinal axis direction index value output by the second index adder, and the second input end of the third index adder is used for receiving the product output by the index multiplier; and the third index adder is used for controlling the addition of the product of the number of the column grids and the sum value output by the first index adder and the index value in the longitudinal axis direction, setting the sum value obtained by the addition as an index value in a storage space for mapping the discrete point cloud to the truncated symbol distance function value, and sending the index value to the interconnection bus so as to read out the truncated symbol distance function value and the weight value matched with the index value from the memory module. Compared with the above preferred example of obtaining the index value by line scanning, the index multiplier controls the number of the columns of grids to be multiplied by the index value in the horizontal axis direction, then the third index adder controls the product of the index value in the vertical axis direction and the output of the index multiplier to be added, and the index value in the storage space where the discrete point cloud is mapped to the truncated symbol distance function value is obtained by calculation in a column scanning query mode and is used as the index value or the address of the linear storage space, so that the interconnection bus can read the truncated symbol distance function value and the weight value which are matched with the index value from the memory module.

It should be noted that the grid map expanded in advance is a grid map obtained by expanding transformation according to an expansion parameter required by a maximum detection radius reached by a currently stored point cloud, an expansion parameter required by executing the coordinate system transformation, an expansion parameter required by executing the rotation transformation, and an expansion parameter required by aligning a bus bit width on the basis of an original point cloud defined map; the above-mentioned extension parameters are also stored in the parameter register group and used as parameters for hardware to execute the rotation and translation of the point cloud, and the above-mentioned extension parameters are all related to the maximum range reached by the point cloud. The original point cloud is all the laser point clouds obtained by scanning the laser radar for one circle currently; the original point cloud defined map is a grid map defined by taking a laser radar as a center and taking the maximum detection diameter reached in the original point cloud as the map side length, or the grid map constructed by default of the system is cut by taking a robot as the center and taking the maximum range reached by the point cloud as the radius; the alignment bus bit width means that the memory of the grid covered by the side length of the grid map expanded in advance is equal to the bit width of a bus for transmitting the grid map; the preset coordinate offset value is determined by a coordinate translation parameter required by the maximum detection radius reached by the currently stored point cloud, a coordinate translation parameter required by executing the coordinate system transformation, and a sum of the coordinate translation parameter required by executing the rotation transformation and a coordinate translation parameter required by aligning the bus bit width. The grid map expanded in advance is expanded according to the memory parameters occupied by the coordinate translation parameter, the coordinate rotation parameter and the map side length on the basis of the maximum coverage radius of the point cloud, so that the actual use size of the map can be constrained, the bit width of a bus used in map transmission is adapted, data alignment is guaranteed, and the storage space occupied by the TSDF map is reduced.

As an embodiment, as can be seen in conjunction with fig. 1 and 3, the point cloud processing module includes a point cloud discretization sub-module; the point cloud discretization sub-module comprises a first discrete adder, a first discrete subtracter, a first discrete multiplier, a second discrete adder, a second discrete subtracter and a second discrete multiplier; the first discrete adder and the second discrete adder both belong to adders, and are circles marked with "+" in the point cloud discretization sub-module of fig. 3; the first discrete subtracter and the second discrete subtracter both belong to subtracters and are represented as circles marked with "-" in the point cloud discretization submodule of fig. 3; the first discrete multiplier and the second discrete multiplier both belong to multipliers, which are represented by circles marked with "x" in the point cloud discretization sub-module of fig. 3.

The first input end of the first discrete adder is configured to receive the abscissa of the point cloud transmitted by the interconnection bus, and preferably, the abscissa of the point cloud transmitted by the interconnection bus is registered in a register inside the point cloud discretization sub-module, and the first input end of the first discrete adder receives the abscissa of the point cloud transmitted by the interconnection bus through a register connected to the first input end of the first discrete adder. In the embodiment shown in FIG. 3, the first input terminal of the first discrete adder (the first adder arranged from top to bottom in the point cloud discretization sub-module of FIG. 3) and the register P _x (is marked with P _x In a register P), wherein the register P _x The system comprises a first discrete adder, a second discrete adder and a third discrete adder, wherein the first discrete adder is used for buffering the abscissa of the point cloud transmitted by the interconnection bus and transmitting the abscissa of the currently buffered point cloud to a first input end of the first discrete adder so that the first input end of the first discrete adder receives the abscissa of the point cloud transmitted by the interconnection bus; the Point Cloud transmitted to the first discrete adder by the interconnection bus is derived from the currently stored Point Cloud stored by the memory module, and particularly derived from the block storage space corresponding to the aforementioned block marked with Point Cloud; the abscissa of the point cloud belongs to the abscissa of the laser point cloud in the laser coordinate system.

A second input of the first discrete adder for receiving the abscissa of the robot position transmitted by said interconnection bus, preferably said abscissaThe abscissa of the robot position transmitted by the interconnection bus is supported to be registered in a register in the point cloud discretization submodule, and the second input end of the first discretization adder receives the abscissa of the robot position transmitted by the interconnection bus through the connected register. In the embodiment shown in FIG. 3, the second input of the first discrete adder is coupled to the register Pose _x (labeled with Pose _x Block of) in which the register Pose _x And the cross coordinate of the robot position transmitted by the interconnection bus is cached, and then the cross coordinate of the robot position cached at present is transmitted to the second input end of the first discrete adder. The abscissa of the robot position is a horizontal axis coordinate value of the robot under a world coordinate system, which is calculated in advance (by software or a CPU unit); the lidar is mounted on the robot and acts as a sensor device for robot positioning. And the first discrete adder is used for controlling the addition of the abscissa of the point cloud and the abscissa of the position of the robot and outputting a sum obtained by the addition so that the sum becomes a coordinate value of the abscissa of the point cloud converted into a world coordinate system.

The first input end of the first discrete subtractor is used for receiving the sum value output by the first discrete adder, and in this embodiment, the first input end of the first discrete subtractor is connected with the output end of the first discrete adder to construct a combinational logic circuit; the second input end of the first discrete subtractor is configured to receive the maximum abscissa value of the map transmitted by the interconnection bus, and preferably, the maximum abscissa value of the map transmitted by the interconnection bus is supported to be registered in a register inside the point cloud discrete sub-module, and the second input end of the first discrete subtractor receives the maximum abscissa value of the map transmitted by the interconnection bus through the connected register. In the embodiment shown in fig. 3, the second input terminal of the first discrete subtractor and the register MAX _x (marked with MAX _x Block of) in which the register MAX is connected, in which _x The device is used for caching the maximum abscissa value of the map transmitted by the interconnection bus and then transmitting the currently cached maximum abscissa value of the map to the second input end of the first discrete subtracter. The map is maximum recumbentThe scalar value is derived from the map size register and transmitted to the interconnect bus by the map size register; the map size register is a parameter register arranged in the correlation scanning matching hardware circuit and used for storing the size range of the grid map meeting the transmission requirement of the bus bit width; the parameter register set shown in fig. 1 includes a map size register. And the first discrete subtracter is used for controlling the maximum abscissa value of the map to be subtracted from the sum value output by the first discrete adder, outputting the difference value obtained by subtraction, and setting the difference value to be a longitudinal coordinate value of the grid map which is transformed into the point cloud meeting the transmission requirement of the bus bit width.

The first input end of the first discrete multiplier is used for receiving the difference value output by the first discrete subtractor, in this embodiment, the first input end of the first discrete multiplier is connected with the output end of the first discrete subtractor to construct a combinational logic circuit; the second input end of the first discrete multiplier is configured to receive the reciprocal of the map resolution transmitted by the interconnection bus, and preferably, the reciprocal of the map resolution transmitted by the interconnection bus is supported to be registered in a register inside the point cloud discrete sub-module, and the second input end of the first discrete multiplier receives the reciprocal of the map resolution transmitted by the interconnection bus through a register connected to the second input end of the first discrete multiplier. In the embodiment shown in fig. 3, the second input terminal of the first discrete multiplier is connected to a register 1/R (a box marked with 1/R), where the register 1/R is used for buffering the inverse number of the map resolution transmitted by the interconnection bus, and then transmitting the inverse number of the currently buffered map resolution to the second input terminal of the first discrete multiplier, so that the second input terminal of the first discrete multiplier receives the inverse number of the map resolution transmitted by the interconnection bus; wherein the inverse of the map resolution is derived from a map resolution register and transmitted by the map resolution register to the interconnect bus; the map resolution register is a parameter register arranged in the correlation scanning matching hardware circuit and used for storing resolution information of the raster map meeting the transmission requirement of the bus bit width; the parameter register set shown in fig. 1 includes a map resolution register. The first discrete multiplier is used for controlling the reciprocal of the map resolution to be multiplied by the difference value output by the first discrete subtractor, outputting a product value obtained by multiplication, and configuring the product value as an abscissa value of the discrete point cloud so as to complete coordinate system transformation and realize that the abscissa value of the currently stored point cloud is aligned to the coordinate system of the grid map meeting the transmission requirement of the bus bit width; the first Discrete multiplier is further configured to output a product value obtained by multiplying to the memory module through the interconnection bus, and store the product value into the block storage space corresponding to the foregoing square frame marked with the Discrete Point Cloud, so that the TSDF value calculation module reads an abscissa value of the Discrete Point Cloud.

The first input end of the second discrete adder is configured to receive the ordinate of the point cloud transmitted by the interconnection bus, and preferably, the ordinate of the point cloud transmitted by the interconnection bus is supported to be registered in a register inside the point cloud discretization sub-module, and the first input end of the second discrete adder receives the ordinate of the point cloud transmitted by the interconnection bus through a register connected to the second input end of the second discrete adder. In the embodiment shown in FIG. 3, the first input terminal of the second discrete adder (the second adder arranged from top to bottom in the point cloud discretization sub-module of FIG. 3) and the register P _y (is marked with P _y Block of) phase, wherein the register P _y The device is used for caching the vertical coordinate of the point cloud transmitted by the interconnection bus, transmitting the vertical coordinate of the currently cached point cloud to the first input end of the second discrete adder, and enabling the first input end of the second discrete adder to finish receiving the vertical coordinate of the point cloud transmitted by the interconnection bus; the Point Cloud transmitted to the second discrete adder by the interconnection bus is derived from the currently stored Point Cloud stored by the memory module, and particularly derived from the block storage space corresponding to the aforementioned square block marked with Point Cloud; the vertical coordinate of the point cloud belongs to the vertical coordinate value of the laser point cloud under the laser coordinate system.

A second input of the second discrete adder for receiving the ordinate of the robot position transmitted by the interconnection bus, preferably the interconnection busThe transmitted vertical coordinate of the robot position is supported to be registered in a register in the point cloud discretization sub-module, and a second input end of a second discretization adder receives the vertical coordinate of the robot position transmitted by the interconnection bus through the connected register. In the embodiment shown in FIG. 3, the second input of the second discrete adder is connected to the register Pose _y (marked with Pose _y Block of) phase, in which the register Pose _y And the device is used for caching the vertical coordinate of the robot position transmitted by the interconnection bus and transmitting the currently cached vertical coordinate of the robot position to the second input end of the second discrete adder. The vertical coordinate of the position of the robot is a pre-calculated vertical coordinate value of the robot in a world coordinate system; and the second discrete adder is used for controlling the addition of the vertical coordinate of the point cloud and the vertical coordinate of the position of the robot and outputting a sum value obtained by the addition so that the sum value becomes a vertical coordinate value of the point cloud converted into a world coordinate system.

A first input end of the second discrete subtracter is configured to receive a sum value output by the second discrete adder, in this embodiment, the first input end of the second discrete subtracter is connected to an output end of the second discrete adder to construct a combinational logic circuit; and the second input end of the second discrete subtracter is used for receiving the maximum longitudinal coordinate value of the map transmitted by the interconnection bus, preferably, the maximum longitudinal coordinate value of the map transmitted by the interconnection bus is supported to be registered in a register inside the point cloud discrete sub-module, and the second input end of the second discrete subtracter receives the maximum longitudinal coordinate value of the map transmitted by the interconnection bus through the connected register. In the embodiment shown in fig. 3, the second input of the second discrete subtractor is connected to the register MAX _y (marked with MAX _y Block of) in which the register MAX is connected, in which _y The device is used for caching the maximum longitudinal coordinate value of the map transmitted by the interconnection bus and then transmitting the maximum longitudinal coordinate value of the map cached currently to the second input end of the second discrete subtracter. The map maximum ordinate value is derived from the map size register and transmitted by the map size register to the interconnect bus. Second discrete subtractionAnd the device is used for controlling the subtraction of the maximum longitudinal coordinate value of the map and the sum value output by the second discrete adder, then outputting the difference value obtained by the subtraction, and setting the difference value as a point cloud to be transformed into the horizontal coordinate value of the grid map meeting the transmission requirement of the bus bit width.

The first input end of the second discrete multiplier is used for receiving the difference value output by the second discrete subtractor, in this embodiment, the first input end of the second discrete multiplier is connected with the output end of the second discrete subtractor to construct a combinational logic circuit; the second input end of the second discrete multiplier is configured to receive the inverse of the map resolution transmitted by the interconnection bus, and preferably, the inverse of the map resolution transmitted by the interconnection bus is supported to be registered in a register inside the point cloud discrete sub-module, and the second input end of the second discrete multiplier receives the inverse of the map resolution transmitted by the interconnection bus through a register connected to the second input end of the second discrete multiplier. In the embodiment shown in FIG. 3, the second input of the second discrete multiplier is connected to register 1/R (the block labeled 1/R); the second discrete multiplier is used for controlling the reciprocal of the map resolution to be multiplied by the difference value output by the second discrete subtracter, outputting a product value obtained by multiplication, configuring the product value as a longitudinal coordinate value of the discrete point cloud to complete coordinate system transformation, and aligning the longitudinal coordinate value of the currently stored point cloud to a coordinate system of the grid map meeting the transmission requirement of the bus bit width; and the second Discrete multiplier is also used for outputting a product value obtained by multiplication to the memory module through the interconnection bus and storing the product value into the block storage space corresponding to the square block marked with the Discrete Point Cloud so as to read the longitudinal coordinate value of the Discrete Point Cloud by the TSDF value calculating module.

Therefore, for each point cloud, the embodiment of the point cloud discretization sub-module reads the horizontal coordinate and the vertical coordinate from the block storage space in which the point cloud is stored, inputs the point cloud discretization sub-module, and transforms the horizontal coordinate and the vertical coordinate of the point cloud into the coordinate system of the pre-expanded grid map in the same offset mode along the coordinate axis direction which is adapted to the point cloud, including the coordinate axis where the point cloud is exchanged, so as to realize the transformation of the coordinate system; and then multiplying the point cloud by the reciprocal of the resolution of the grid map to complete the discretization processing of the point cloud.

On the basis of the above embodiment, as can be seen from fig. 1 and 3, the point cloud processing module further includes a point cloud rotation sub-module; the point cloud rotation submodule comprises 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 subtracter; the first point cloud multiplier, the second point cloud multiplier, the third point cloud multiplier and the fourth point cloud multiplier all belong to multipliers, and are represented as circles marked with 'x' in the point cloud rotation sub-modules in the figure 3; the point cloud adder belongs to an adder and is represented as a circle marked with "+" in the point cloud rotation submodule of FIG. 3; the point cloud subtracter belongs to a subtracter and is represented as a circle marked with a mark in a point cloud rotation submodule of fig. 3.

And the first input end of the first point cloud multiplier is used for receiving data with the same horizontal coordinate as the point cloud received by the first input end of the first discrete adder, a first register is arranged in the point cloud discretization submodule, and the first input end of the first point cloud multiplier is connected with one output end of the first register. The abscissa of the point cloud transmitted by the interconnection bus is supported to be registered in a first register inside the point cloud discretization sub-module, and in the embodiment shown in fig. 3, a first input end of a first point cloud multiplier (a first multiplier arranged from top to bottom in the point cloud rotation sub-module of fig. 3) and a register P are provided _x (labeled with P) _x Block of (d) is connected to one output of the register P _x (is marked with P _x Block of) is connected to a first input of said first discrete adder, wherein register P is connected to a first input of said first discrete adder _x Is the first register. Therefore, the abscissa of the point cloud transmitted to the first input end of the first discrete adder by the interconnection bus is firstly cached in the first register, and then is output to the first input end of the first discrete adder and the first input end of the first point cloud multiplier by the first register.

The second input end of the first point cloud multiplier is used for receiving a cosine function value corresponding to a rotation angle reached by the current rotation transformation, wherein the rotation angle reached by the current rotation transformation is a sum of an angle search step length and a rotation angle reached by the last rotation transformation, and is derived from the rotation angle obtained by stepping rotation operation executed on a CPU or software level and the cosine function value corresponding to the rotation angle, so that the complexity of a hardware unit caused by trigonometric function operation is avoided, and hardware transmission delay is prevented from being caused. Preferably, the cosine function value transmitted by the interconnection bus is supported to be registered in a 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. In the embodiment shown in fig. 3, the second input terminal of the first blob multiplier is connected to the output terminal of a register COS (box labeled with COS), wherein the register COS is used for buffering the cosine function value transmitted from the interconnect bus, and then transmitting the currently buffered cosine function value to the second input terminal of the first blob multiplier. And the first point cloud multiplier is used for controlling the abscissa of the point cloud received by the first input end of the first point cloud multiplier to be multiplied by the cosine function value corresponding to the rotation angle reached by the current rotation transformation received by the second input end of the first point cloud multiplier, and outputting the product value obtained by multiplication.

The first input end of the second point cloud multiplier is used for receiving data with the same vertical coordinate as the point cloud received by the first input end of the second discrete adder; the point cloud discretization submodule is internally provided with a second register, and a first input end of a second point cloud multiplier is connected with one output end of the second register. The vertical coordinate of the point cloud transmitted from the interconnection bus is supported and registered in the second register inside the point cloud discretization sub-module, in the embodiment shown in fig. 3, the first input end of the second point cloud multiplier (the second multiplier arranged from top to bottom in the point cloud rotation sub-module of fig. 3) and the register P _y (labeled with P) _y Block of (d) is connected to one output of the register P _y (is marked with P _y Block of) is connected to a first input of said second discrete adder, wherein register P is connected to a first input of said second discrete adder _y Is the second register. Therefore, the vertical coordinate of the point cloud transmitted to the first input end of the second discrete adder by the interconnection bus is firstly cached in the second register, and then is output to the first input end of the second point cloud multiplier and the first input end of the second discrete adder by the second register.

The second input end of the second point cloud multiplier is used for receiving a sine function value corresponding to the rotation angle reached by the current rotation transformation; the rotation angle achieved by the current rotation transformation is the sum of the angle search step length and the rotation angle achieved by the last rotation transformation, and is derived from the rotation angle obtained by stepping rotation operation executed on a CPU or software level and a corresponding sine function value, so that the complexity of a hardware unit increased by trigonometric function operation is avoided, and hardware transmission delay is prevented from being caused. Preferably, the sine function value transmitted by the interconnection bus is supported to be registered in a register inside the point cloud rotation submodule, and a second input end of the second point cloud multiplier receives the sine function value transmitted by the interconnection bus through a connected register. In the embodiment shown in fig. 3, the second input terminal of the second point cloud multiplier is connected to the output terminal of a register SIN (a box labeled with SIN), where the register SIN is used to buffer the sine function value transmitted by the interconnection bus, and then transmit the currently buffered sine function value to the second input terminal of the second point cloud multiplier. And the second point cloud multiplier is used for controlling the vertical coordinate of the point cloud received by the first input end of the second point cloud multiplier to be multiplied by a sine function value corresponding to the rotation angle reached by the current rotation transformation received by the second input end of the second point cloud multiplier, and outputting a product value obtained by multiplication.

A first input end of the third point cloud multiplier, configured to receive the abscissa of the point cloud output by the first register, which is the abscissa of the point cloud currently received by the first input end of the first discrete adder, in the implementation shown in fig. 3In the example, the first input terminal of the third point cloud multiplier (the third multiplier arranged from top to bottom in the point cloud rotation sub-module of FIG. 3) and the register P _x (labeled with P) _x Block) of the register P, wherein the register P is connected to one output of the register P _x Is the first register. Therefore, the abscissa of the point cloud transmitted to the first input end of the first discrete adder by the interconnection bus is firstly cached in the first register, and then is output to the first input end of the third point cloud multiplier and the first input end of the first point cloud multiplier by the first register.

The second input end of the third point cloud multiplier is used for receiving a sine function value corresponding to the rotation angle reached by the current rotation transformation; preferably, the sine function value transmitted by the interconnection bus is supported to be registered in a register inside the point cloud rotation submodule, and a second input end of the second point cloud multiplier receives the sine function value transmitted by the interconnection bus through a connected register. In the embodiment shown in fig. 3, the second input terminal of the second point cloud multiplier is connected to the output terminal of a register SIN (a box labeled with SIN), where the register SIN is used to buffer the sine function value transmitted from the interconnection bus, and then transmit the currently buffered sine function value to the second input terminal of the third point cloud multiplier. And the third point cloud multiplier is used for controlling the vertical coordinate of the point cloud currently transmitted by the interconnection bus to be multiplied by the sine function value corresponding to the rotation angle reached by the current rotation transformation, and outputting the product value obtained by multiplication. And the third point cloud multiplier is used for controlling the abscissa of the point cloud received by the first input end of the third point cloud multiplier to be multiplied by the sine function value corresponding to the rotation angle reached by the current rotation transformation received by the second input end of the third point cloud multiplier, and then outputting the product value obtained by multiplication.

A first input end of a fourth point cloud multiplier, which is used for receiving the vertical coordinate of the point cloud output by the second register, wherein 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 end of the second discrete adder, and the vertical coordinate is shown in the figure3, the first input terminal of the fourth cloud multiplier (the fourth multiplier arranged from top to bottom in the point cloud rotation submodule of fig. 3) and the register P _y (is marked with P _y Block) of the register P, wherein the register P is connected to one output of the register P _y Is the second register. Therefore, the ordinate of the point cloud transmitted by the interconnection bus to the first input end of the second discrete adder is firstly cached in the second register, and then is output to the first input end of the fourth cloud multiplier and the first input end of the second discrete adder by the second register.

In the embodiment shown in fig. 3, the second input terminal of the fourth point cloud multiplier is connected to an output terminal of a register COS (a box marked with COS), where the register COS is used to buffer the cosine function value transmitted by the interconnect bus, and then transmit the currently buffered cosine function value to the second input terminal of the fourth point cloud multiplier. And the fourth cloud multiplier is used for controlling the vertical coordinate of the point cloud received by the first input end of the fourth cloud multiplier to be multiplied by a cosine function value corresponding to the rotation angle reached by the current rotation transformation received by the second input end of the fourth cloud multiplier, and outputting the product value obtained by multiplication.

The first input end of the point cloud subtracter is used for receiving the product value output by the first point cloud multiplier; the second input end of the point cloud subtracter is used for receiving the product value output by the second point cloud multiplier; in this embodiment, a first input end of the point cloud subtractor is connected with an output end of the first point cloud multiplier, and a second input end of the point cloud subtractor is connected with an output end of the second point cloud multiplier, so as to construct a combinational logic circuit; the point cloud subtracter is used for controlling the product value output by the first point cloud multiplier to subtract the product value output by the second point cloud multiplier, outputting the subtracted difference value and setting the subtracted difference value as an abscissa value of the rotating point cloud obtained after the point cloud is subjected to current rotation transformation; the Point Cloud subtracter is also used for transmitting the output difference value to the memory module through the interconnection bus and covering the abscissa of the Point Cloud stored in the memory module, namely, the difference value output by the Point Cloud subtracter updates the Point Cloud in the block storage space corresponding to the square block marked with the Discrete Point Cloud so as to update the abscissa result converted from the current rotation into the abscissa of the currently stored Point Cloud stored in the memory module.

The first input end of the point cloud adder is used for receiving the product value output by the third point cloud multiplier; the second input end of the point cloud adder is used for receiving the product value output by the fourth point cloud multiplier; in this embodiment, a first input end of the point cloud adder is connected to an output end of the third point cloud multiplier, and a second input end of the point cloud adder is connected to an output end of the fourth point cloud multiplier, so as to construct a combinational logic circuit; the point cloud adder is used for controlling the addition of the product value output by the third point cloud multiplier and the product value output by the fourth point cloud multiplier, outputting the sum value obtained by the addition, and setting the sum value as a longitudinal coordinate value of the rotating point cloud obtained after the point cloud is subjected to current rotation transformation; the Point Cloud adder is also used for transmitting the output difference value to the memory module through the interconnection bus and covering the vertical coordinate of the Point Cloud stored in the memory module, namely, the difference value output by the Point Cloud subtracter updates the Point Cloud in the block storage space corresponding to the square block marked with the Discrete Point Cloud so as to update the vertical coordinate result converted from the current rotation to the vertical coordinate of the currently stored Point Cloud stored in the memory module.

It should be noted that the angle search step length is stored in a search window parameter register, and the search window parameter register is a parameter register set inside the correlation scan matching hardware circuit and used for storing the pose covered by the search window and the associated step search information; the parameter register set shown in fig. 1 includes the search window parameter register. The rotation times are determined by the angle search range of the search window which is configured in advance and the angle search step length which is configured in advance, so that all poses in the search window can be traversed (inquired) within the corresponding rotation times. Each rotation transformation corresponds to one coordinate system transformation executed by the point cloud processing module and belongs to one matching processing of correlation scanning matching of the map.

In summary, the embodiment of the point cloud rotation sub-module uses the rotation matrix as a basic operation architecture, and only uses an adder, a subtractor and a multiplier to construct the point cloud rotation sub-module for receiving and processing the trigonometric function value corresponding to the step rotation angle calculated in advance by the software, so that not only is a hardware calculation module for trigonometric function omitted, but also the point cloud rotation sub-module is controlled to perform the rotation transformation iteratively on the basis of step rotation to obtain the point cloud for updating the original memory in the memory module. According to the technical scheme, the traversal of each pose of the search window is completed by combining the point cloud discretization sub-module, so that the TSDF value calculating module is promoted to construct a weighting function of each pose of the search window.

In one embodiment, the state machine control module is a finite state machine; the state machine control module is used for scheduling the working states of the memory module, the point cloud processing module and the TSDF value calculating module, so that when the point cloud processing module executes the coordinate system transformation and the rotation transformation once, the TSDF value calculating module executes the division operation once to obtain a TSDF value matched with one pose corresponding to the searching in the searching window, and therefore on the basis of updating the currently stored point cloud by using the result obtained by the rotation transformation, the point cloud processing module is controlled to continuously execute the coordinate system transformation, the TSDF value calculating module is controlled to execute the division operation again until the TSDF value calculating module counts all poses in the searching window and calculates all TSDF values matched with the poses in sequence. In this embodiment, the state machine control module finishes correlation scan matching corresponding to the pose in the search window in a way of scheduling a working state signal in hardware, and includes that on the premise that the TSDF value calculation module calculates the TSDF value of one pose in the search window every time, the point cloud processing module is controlled to sequentially perform rotation-translation transformation on each point cloud, so as to read out a truncated symbol distance function value and a weight value matched with an index value from the memory module, thereby realizing operation in a cyclic state, ensuring hardware normal operation of correlation scan matching, and accelerating the convergence speed of an algorithm.

In this embodiment, the correlation scan matching hardware circuit is implemented by a finite state machine to control and run a correlation scan matching algorithm, so as to implement the calculation of the TSDF value after the point cloud is aligned to the grid map, where the finite state machine includes 12 working states, which are: the system comprises an idle state, a point cloud reading state, a truncation symbol distance function reading state, a weight value reading state, a resetting reading state, a point cloud rasterizing state, a rasterizing resetting state, a searching state, a dividing state, a circulating state, a division finishing waiting state and a data writing state.

The initial state is an idle state, the working state of the state machine control module jumps to a Point Cloud reading state, and at the moment, the state machine control module is used for controlling the memory module to read the currently stored Point Cloud from an external FIFO (first in first out) and then store the read Point Cloud into a first block storage space, namely, the external FIFO reading module transmits the Point Cloud to a block storage space corresponding to a square frame marked with Point Cloud; if the currently collected circle of point clouds are all read, the state jumps to a state of reading a truncation symbol distance function value, at the moment, the memory module is controlled to read the truncation symbol distance function value from an external reading FIFO, and then the read truncation symbol distance function value is stored in a second storage space, namely, the external reading FIFO transmits the point clouds to a position marked with Map of tsdf _i (x) In the block storage space corresponding to the square frame of (1); after all the truncation symbol distance function values are read, the state is jumped to a weight value reading state, the memory module is controlled to read the weight value matched with the truncation symbol distance function values from the external FIFO, and then the read weight value is stored in a third block of storage space, namely, the external FIFO is used for transmitting the point cloud to a position marked with Map of weight _i (x) Is stored in the corresponding block storage space. After all weight values are read into the third block storage space, the working state is jumped to readAnd taking a reset state, controlling the read address of the memory module to be reset, simultaneously controlling the write address of the memory module to start self-adding, and opening up a fourth storage space so that the coordinate system conversion result output by the point cloud processing module is written into an empty block storage space existing in the memory module.

After the write address of the memory module completes the address setting in a self-adding mode, the working state jumps to a point cloud rasterization state, the point cloud processing module is controlled to execute the coordinate system transformation, and then the result of the coordinate system transformation is controlled to be written into the fourth storage space through the interconnection bus. When the results obtained by the current coordinate system transformation in the correlation scanning matching of the map are all written into the fourth storage space, the working state jumps to a rasterized reset state, the read address and the write address of the first storage space are controlled to be reset, the read address of the fourth storage space is controlled to be reset, the write address of the fourth storage space is controlled to be started to be added, so that an empty address space is continuously opened up in the fourth storage space, and the empty address space is used for storing the results obtained by the next coordinate system transformation; and the point cloud participating in the next coordinate system transformation is a rotation transformation result obtained by the point cloud processing module executing rotation transformation while the point cloud processing module executes the current coordinate system transformation. After setting an address space for a result obtained by next coordinate system transformation in the fourth storage space, jumping to a search state from a working state, controlling the TSDF value calculation module to calculate an index value of the discrete point cloud mapped into the storage space of the truncated symbol distance function value, and reading the truncated symbol distance function value and a weight value matched with the truncated symbol distance function value from the storage module according to the index value; after the calculation is completed, the working state jumps to a division state, and then according to the foregoing embodiment, the division operation is performed once based on the read truncated symbol distance function value and the weight value matched thereto, so as to obtain a TSDF value corresponding to a pose matching searched in the search window, and then the TSDF value obtained by the operation is transmitted to the interconnection bus.

After division calculation is completed, the working state jumps to a circulating state, at the moment, the value of a circulating register is set according to the coordinate index value of the current searching pose in the searching window, if the searching of all the poses in the searching window is judged not to be completed, the working state jumps to the point cloud rasterization state, the point cloud processing module is controlled to execute the coordinate system transformation again to repeat the working state, therefore, the point cloud processing module controls the TSDF value calculation module to execute division calculation once every time the point cloud processing module executes the coordinate system transformation and the rotation transformation once to obtain a TSDF value which is matched with one corresponding searched pose in the searching window, and the TSDF value obtained by calculation is transmitted to the interconnection bus.

And if the searching of all the poses in the searching window is judged to be completed, the working state jumps to a division completion waiting state, and the division completion of the last time is waited. And after the last division is finished, the working state jumps to a data writing state, namely the TSDF value calculation module counts all existing poses in the searched search window, then all TSDF values obtained by operation are written into an external write FIFO through the interconnection bus, and after the data writing is finished, the working state jumps back to an idle state to wait for the start of the next accelerated calculation.

As an embodiment, a bus interface module is arranged outside the correlation scanning matching hardware circuit, and the bus interface module comprises a DMA controller module and a transmission bus; preferably, the bus interface module includes an AXI DMA module and an AXI bus.

The DMA controller module is used for continuously transmitting data stored in a physical storage space of a discontinuous address in batches, and reducing the triggering times of software interrupt of a CPU; specifically, the DMA controller module needs a device driver of the CPU to generate a linked list storing data addresses, describes a discontinuous physical space with the linked list, and sends an initial address of the linked list to the DMA controller module, and after the DMA controller module has finished transmitting one piece of data to the correlation scan matching hardware circuit, the DMA controller module sequentially reads an address of a next linked list until all data is transmitted.

The transmission bus comprises a first bus and a second bus, preferably the transmission bus is an AXI bus, comprising two AXI buses, an AXI-Lite bus and an AXI-Stream bus, respectively. The former is suitable for simple, low throughput memory-mapped communications, and the latter is for high-speed streaming data. The first bus is preferably an AXI-Lite bus, the processor unit, the correlation scanning matching hardware circuit and the DMA controller module are connected, control signals can be sent to the memory module, the point cloud processing module, the TSDF value calculation module, the state machine control module and the interconnection bus, and the working state of the state machine control module is monitored and read; the first bus is in signal transceiving contact 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, and is used for configuring data transmission parameters for the DMA controller module, and also used for configuring parameters stored in the map size register, parameters stored in the map resolution register, expansion parameters required by the maximum detection radius reached by the currently stored point cloud, expansion parameters required by executing the coordinate system transformation, expansion parameters required by executing the rotation transformation, expansion parameters required by meeting the transmission requirement of the bus bit width and search step length so as to realize mapping communication of the memory of the correlation scanning matching hardware circuit. The second bus is preferably an AXI-Stream bus, is connected with the DMA controller module and is used for transmitting a truncation symbol distance function which is configured in advance by the CPU and a weight value matched with the truncation symbol distance function to the memory module; wherein the transmission bus is in compliance with AMBA protocol. The embodiment provides a bus interface architecture module for the correlation scan matching hardware circuit and an external data source (controller), and designs a first bus suitable for simple and low-throughput memory mapping communication (transmitting an extension parameter, a map feature parameter and an associated basic control signal to a register inside the correlation scan matching hardware circuit) and a second bus for high-speed data flow (transmitting a truncated symbol distance function value and a weight value to the memory module) according to data transmission performance; the real-time performance of the operation work of the correlation scanning matching hardware circuit is improved.

Based on the foregoing embodiment, the present invention further discloses a heterogeneous computing architecture, which includes a processor unit, a bus interface, and the correlation scan matching hardware circuit disclosed in the foregoing embodiment, where the processor unit and the correlation scan matching hardware circuit are interconnected through the bus interface, the processor unit does not control parallel acceleration operation performed by the correlation scan matching hardware circuit, and the processor unit only provides an operation parameter, a start signal, an interrupt signal, a reset signal, and a flag bit clearing signal for 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 circuitry; 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 makes a balance between accuracy and cost; the correlation scan matching hardware circuit is adapted for data parallel algorithms such that the acceleration obtained by the heterogeneous architecture facilitates real-time processing of high frame rate point cloud maps.

While the invention has been described with reference to a preferred embodiment, it will be understood by those skilled in the art that various changes may be made and equivalents may be substituted for elements thereof without departing from the scope of the invention. However, any simple modification, equivalent change and modification of the above embodiments according to the technical essence of the present invention will still fall within the protection scope of the technical solution of the present invention.

Claims (11)

1. The correlation scanning matching hardware circuit of the TSDF map is characterized in that the correlation scanning matching hardware circuit is electrically connected with a laser radar;

   the correlation scanning matching hardware circuit comprises a memory module, a point cloud processing module, a TSDF value calculating module, a state machine control module and an interconnection bus, wherein the memory module, the point cloud processing module, the TSDF value calculating module and the state machine control module establish a data transmission relation through the interconnection bus;

   the point cloud processing module is used for reading the currently stored point cloud from the memory module under the control of the state machine control module, controlling the read point cloud to execute coordinate system transformation, setting the result of the coordinate system transformation as discrete point cloud, and aligning the currently stored point cloud into the coordinate system of the pre-expanded grid map; then storing the result of the coordinate system transformation into a memory module;

   the point cloud processing module is also used for reading the currently stored point cloud from the memory module under the control of the state machine control module, controlling the read point cloud to execute rotation transformation, updating the result of the rotation transformation into the currently stored point cloud stored in the memory module, controlling the updated point cloud to execute the coordinate system transformation and determining the primary matching processing for finishing the correlation scanning matching of the map; each matching process of the correlation scanning matching of the map comprises a coordinate system transformation;

   the TSDF value calculating module is used for reading the discrete point cloud from the memory module under the control of the state machine control module, calculating an index value of the discrete point cloud mapped to a storage space of the truncated symbolic distance function value according to a preset coordinate offset value and a search step length, and setting the index value as a reading address of the truncated symbolic distance function value and a reading address of the weight value stored in the memory module; the truncation symbol distance function value and the weight value both have a matched index value;

   the TSDF value calculating module is also used for reading a truncated symbol distance function value and a matched weight value thereof from the memory module according to the currently set reading address under the control of the state machine control module, then controlling a divider arranged in the TSDF value calculating module to equivalently complete the weighted average processing of the truncated symbol distance function value mapped by the currently stored point cloud in a division operation mode, and setting the result output by each operation of the divider as a TSDF value for representing the positioning precision of a corresponding pose searched in the search window; when the point cloud processing module finishes one matching process of the map correlation scanning matching, a divider built in the TSDF value calculating module executes one division operation to obtain a TSDF value corresponding to a new pose in a search window;

   the memory module is used for storing the currently stored point cloud, the discrete point cloud obtained by the currently stored point cloud through the transformation of the point cloud processing module, the preset truncated symbol distance function and the matched weight value thereof; wherein each type of data is stored in a different block of storage space of the same memory module.
2. 2. The correlation scan matching hardware circuit of claim 1, wherein said TSDF value calculating module comprises a selector, a divide-by counter, a pre-calculation unit, a divider, a first accumulator, a second accumulator, a third accumulator;

   the TSDF value calculating module is used for sending the truncated symbol distance function value to a first accumulator and sending the weight value to a second accumulator under the control of the state machine control module, sending the truncated symbol distance function value and the matched weight value to a third accumulator and controlling each accumulator to simultaneously execute accumulation operation; the input ends of the three accumulators are configured to receive data sequentially transmitted by the interconnection bus, and the third accumulator is used for outputting the accumulated sum of the products of the truncated symbol distance function values and the matched weight values;

   the output ends of the three accumulators are respectively connected with the corresponding data input ends of the selector, the selection input end of the selector is connected with the counting output end of the frequency division counter, and the selector is provided with a data output end matched with each data input end and used for gating and outputting data received by the data input end of the selector under the counting triggering action of the frequency division counter so as to adapt to the enabling time sequence of the divider;

   each data output end of the selector is respectively connected with the corresponding input end of the pre-calculation unit;

   the pre-calculation unit comprises a numerator adder, a first numerator subtracter, a second numerator subtracter, a numerator shift register and a denominator shift register; the numerator adder belongs to the adder, the first numerator subtracter and the second numerator subtracter belong to the subtracters, and the numerator shift register and the denominator shift register belong to the shift registers;

   the first input end of the second sub-subtracter is connected with the output end of the first accumulator, and the second input end of the second sub-subtracter is connected with the output end of the third accumulator; the second sub-subtracter is used for controlling the accumulated sum value output by the first accumulator to be subtracted from the accumulated sum value output by the third accumulator and outputting the difference value of the two subtracted data;

   the first molecular subtracter is provided with an input end for receiving the quantity value of the currently stored point cloud transmitted by the interconnection bus; the other input end of the first molecular subtracter is connected with the output end of the second accumulator; the first molecular subtracter is used for controlling 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 outputting the difference value of the two subtracted data; the input end of the molecular shift register is connected with the output end of the first molecular subtracter, and the molecular shift register is used for shifting the difference value output by the first molecular subtracter, so that the bit width of binary number obtained by shifting the difference value output by the first molecular subtracter is equal to the bit width of the accumulated sum value output by the first accumulator;

   the two input ends of the molecular adder are respectively connected with the output end of the molecular shift register and the output end of the second molecular subtracter, and the molecular adder is used for adding the shift output result of the molecular shift register to the difference value output by the second molecular subtracter and then configuring the sum value of the two added data into a dividend;

   the input end of the denominator shift register is connected with the output end of the first sub-subtracter, and the denominator shift register is used for shifting the difference value output by the first sub-subtracter, so that the bit width of the binary number obtained by shifting the difference value output by the first sub-subtracter is one bit lower than the bit width of the binary number of the accumulated sum value output by the first accumulator;

   and two input ends of the divider are respectively connected with the output end of the numerator adder and the output end of the denominator shift register, and the divider is used for controlling a dividend output by the numerator adder and a divisor output by the denominator shift register to perform division operation once after enabling, then sending the operated quotient value back to the interconnection bus, determining that the quotient value becomes a TSDF value corresponding to one pose in the search window, and also determining that the divider equivalently completes one weighted average processing of a truncation symbol distance function value of the currently stored point cloud mapping by executing one division operation.
3. 3. The correlation scan matching hardware circuit of claim 1, wherein the TSDF value computation module comprises a grid indexing submodule comprising a first index subtractor, a second index subtractor, a first index adder, a second index adder, a third index adder, an index multiplier; the first index subtracter and the second index subtracter belong to subtracters, and the first index adder and the second index adder belong to adders;

   the first input end of the first index subtracter is used for receiving the abscissa of the discrete point cloud transmitted by the interconnection bus, wherein the abscissa of the discrete point cloud transmitted by the interconnection bus to the first index subtracter is derived from the memory module;

   the second input end of the first index subtracter is used for receiving the abscissa coordinate deviation value transmitted by the interconnection bus, wherein the preset coordinate deviation value comprises the abscissa coordinate deviation value, and the abscissa coordinate deviation value transmitted by the interconnection bus is derived from the preset coordinate deviation value stored in the map deviation value register; the map offset register is a parameter register arranged in the correlation scanning matching hardware circuit and used for storing a coordinate offset value associated with the pre-expanded grid map;

   the first input end of the first index adder is connected with the output end of the first index subtracter; the second input end of the first index adder is used for receiving the horizontal axis coordinate searching step length transmitted by the interconnection bus; the searching step length comprises a horizontal axis coordinate searching step length, and the horizontal axis coordinate searching step length transmitted by the interconnection bus is derived from the searching step length stored in a searching window parameter register; the search window parameter register is a parameter register arranged in the correlation scanning matching hardware circuit and used for storing the pose 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 a storage space for mapping the discrete point cloud to a truncated symbol distance function value;

   the first input end of the second index subtracter is used for receiving the vertical coordinate of the discrete point cloud transmitted by the interconnection bus, wherein the vertical coordinate of the discrete point cloud transmitted by the interconnection bus to the second index subtracter is from the memory module;

   a second input end of the second index subtracter is configured to receive a vertical axis coordinate offset value transmitted by the interconnection bus, where the preset coordinate offset value further 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 with the output end of the second index subtracter; a second input end of the second index adder is used for receiving a longitudinal axis coordinate searching step length transmitted by the interconnection bus; the search step length also comprises a longitudinal axis coordinate search step length, and the longitudinal axis coordinate search step length transmitted by the interconnection bus is also derived from the search step length stored in the search window parameter register; the sum value output by the second index adder is configured as a longitudinal axis direction index value in a memory space that maps the discrete point cloud to a truncated symbol distance function value.
4. 4. The correlation scan matching hardware circuit of claim 3, wherein said grid index sub-module further comprises a third index adder, an index multiplier, wherein 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 with the output end of the second index adder; a second input end of the index multiplier is used for receiving the row grid quantity transmitted by the interconnection bus, wherein the row grid quantity is the grid quantity which can be occupied by each row of the storage space of the truncated symbol distance function value, is stored in the map size register and is transmitted to the interconnection bus by the map size register; the map size register is a parameter register arranged in the correlation scanning matching hardware circuit and is used for storing the size range of the pre-expanded grid map and associated expanded information;

   the first input end of the third index adder is connected with the output end of the first index adder, the second input end of the third index adder is connected with the output end of the index multiplier, the third index adder is used for controlling the product of the row grid quantity and the sum value output by the second index adder to be added with the index value in the direction of the transverse axis, then the sum value obtained by adding is set as the index value of the discrete point cloud mapped into the storage space of the truncated symbol distance function value, and the index value is sent to the interconnection bus, so that the truncated symbol distance function value and the weighted value matched with the index value can be read from the memory module.
5. 5. The correlation scan matching hardware circuit of claim 3, wherein the grid index sub-module further comprises 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 with the output end of the first index adder; the second input end of the index multiplier is used for receiving the number of the column grids transmitted by the interconnection bus; the column grid number is the number of grids which can be occupied by each column of the storage space of the truncated symbol 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 arranged in the correlation scanning matching hardware circuit and used for storing the size range of the grid map expanded in advance and associated expanded information;

   the first input end of the third index adder is connected with the output end of the second index adder, and the second input end of the third index adder is connected with the output end of the index multiplier; and the third index adder is used for controlling the addition of the product of the number of the column grids and the sum value output by the first index adder and the index value in the longitudinal axis direction, setting the sum value obtained by the addition as an index value in a storage space for mapping the discrete point cloud to the truncated symbol distance function value, and sending the index value to the interconnection bus so as to read out the truncated symbol distance function value and the weight value matched with the index value from the memory module.
6. 6. The correlation scan matching hardware circuit according to claim 1, wherein the pre-expanded grid map is a grid map obtained by expanding a transform according to an expansion parameter required for a maximum detection radius reached by a currently stored point cloud, an expansion parameter required for performing the coordinate system transform, an expansion parameter required for performing the rotational transform, and an expansion parameter required for aligning a bus bit width, in sequence, on the basis of a map defined by an original point cloud;

   the original point clouds are all the laser point clouds obtained by scanning the laser radar for one circle currently; the map defined by the original point cloud is a grid map which is defined by taking a laser radar as a center and taking 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 grid map expanded in advance is equal to the bit width of a bus for transmitting the grid map;

   the preset coordinate deviation value is determined by a sum of a coordinate translation parameter required by the maximum detection radius reached by the currently stored point cloud, a coordinate translation parameter required by the execution of the coordinate system transformation, a coordinate translation parameter required by the execution of the rotation transformation and a coordinate translation parameter required by the alignment of the bus bit width.
7. 7. The correlation scan matching hardware circuit of claim 6, wherein said point cloud processing module comprises a point cloud discretization sub-module;

   the point cloud discretization sub-module comprises a first discrete adder, a first discrete subtracter, a first discrete multiplier, a second discrete adder, a second discrete subtracter and a second discrete multiplier, wherein the first discrete adder and the second discrete adder belong to the adders, the first discrete subtracter and the second discrete subtracter belong to the subtracters, and the first discrete multiplier and the second discrete multiplier belong to the multipliers;

   the first input end of the first discrete adder is used for receiving the abscissa of the 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; the abscissa of the point cloud belongs to the abscissa value under the laser coordinate system;

   the second input end of the first discrete adder is used for receiving the abscissa of the robot position transmitted by the interconnection bus, wherein the abscissa of the robot position is a pre-calculated abscissa value of the robot in a world coordinate system;

   the first discrete adder is used for controlling the addition of the abscissa of the point cloud and the abscissa of the position of the robot and outputting a sum obtained by the addition so that the sum becomes a horizontal axis coordinate value of the point cloud converted into a world coordinate system;

   the first input end of the first discrete subtracter is connected with the output end of the first discrete adder; the second input end of the first discrete subtracter is used for receiving the maximum abscissa value of the map transmitted by the interconnection bus, and the maximum abscissa value of the map is derived from the map size register and is transmitted to the interconnection bus by the map size register; the map size register is a parameter register arranged in the correlation scanning matching hardware circuit and used for storing the size range of the grid map meeting the transmission requirement of the bus bit width; the difference value output by the first discrete subtracter becomes the longitudinal coordinate value of the grid map which meets the transmission requirement of the bus bit width;

   the first input end of the first discrete multiplier is connected with the output end of the first discrete subtracter; the second input end of the first discrete multiplier is used for receiving the reciprocal of the map resolution transmitted by the interconnection bus, and the reciprocal of the map resolution is derived from a map resolution register and is transmitted to the interconnection bus by the map resolution register; the map resolution register is a parameter register arranged in the correlation scanning matching hardware circuit and used for storing resolution information of the raster map meeting the transmission requirement of the bus bit width; the product value output by the first discrete multiplier is configured to be the abscissa value of the discrete point cloud so as to complete coordinate system transformation, and the aim of aligning the abscissa value of the currently stored point cloud into the coordinate system of the grid map meeting the transmission requirement of the bus bit width is achieved; the first discrete multiplier is also used for outputting a product value obtained by multiplying to the memory module through the interconnection bus so as to prepare for the TSDF value calculation module to read the abscissa value of the discrete point cloud;

   the first input end of the second discrete adder is used for receiving the vertical coordinate of the point cloud transmitted by the interconnection bus, wherein the point cloud transmitted by the interconnection bus to the second discrete adder is from the currently stored point cloud stored in the memory module; the vertical coordinate of the point cloud belongs to the vertical coordinate value under the laser coordinate system;

   the second input end of the second discrete adder is used for receiving 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 coordinate value of the robot in a world coordinate system;

   the second discrete adder is used for controlling the addition of the vertical coordinate of the point cloud and the vertical coordinate of the position of the robot and outputting a sum value obtained by the addition so that the sum value becomes a vertical axis coordinate value of the point cloud converted into a world coordinate system;

   the first input end of the second discrete subtracter is connected with the output end of the second discrete adder; a second input end of the second discrete subtractor, configured to receive a map maximum ordinate value transmitted by the interconnection bus, where the map maximum ordinate value is derived from the map size register and is transmitted to the interconnection bus by the map size register; the difference value output by the second discrete subtracter becomes the abscissa value of the grid map which meets the transmission requirement of the bus bit width;

   the first input end of the second discrete multiplier is connected with the output end of the second discrete subtracter; the second input end of the second discrete multiplier is used for receiving the reciprocal of the map resolution transmitted by the interconnection bus; the product value output by the second discrete multiplier is configured to be a longitudinal coordinate value of the discrete point cloud so as to complete coordinate system transformation and align the longitudinal coordinate value of the currently stored point cloud into a coordinate system of a grid map meeting the transmission requirement of bus bit width; and the second discrete multiplier is also used for outputting the product value obtained by multiplication to the memory module through the interconnection bus so as to prepare for the TSDF value calculation module to read the longitudinal coordinate value of the discrete point cloud.
8. 8. The correlation scan matching hardware circuit of claim 7, wherein said point cloud processing module further comprises a point cloud rotation sub-module;

   the point cloud rotation sub-module comprises 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 subtracter, wherein the first point cloud multiplier, the second point cloud multiplier, the third point cloud multiplier and the fourth point cloud multiplier belong to multipliers, the point cloud adder belongs to the adder, and the point cloud subtracter belongs to the subtracter;

   the first input end of the first point cloud multiplier is connected with the output end of the first register; a first register is arranged in the point cloud discretization submodule; the abscissa of the point cloud transmitted to the first input end of the first discrete adder by the interconnection bus is cached to a first register;

   the second input end of the first point cloud multiplier is used for receiving a cosine function value corresponding to a rotation angle reached by the current rotation transformation, wherein the rotation angle reached by the current rotation transformation is the sum of an angle search step length and the rotation angle reached by the last rotation transformation; the angle searching step length is stored in a searching window parameter register; the search window parameter register is a parameter register arranged in the correlation scanning matching hardware circuit and used for storing the pose covered by the search window and the associated stepping search information;

   the first input end of the second point cloud multiplier is connected with the output end of the first register; a second register is arranged in the point cloud discretization submodule; 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 a second register;

   the second input end of the second point cloud multiplier is used for receiving a sine function value corresponding to the rotation angle reached by the current rotation transformation;

   a first input end of the third clouding multiplier is connected with an output end of the first register;

   a second input end of the third point cloud multiplier, configured to receive a sine function value corresponding to the rotation angle reached by the current rotation transformation;

   the first input end of the fourth point cloud multiplier is connected with the output end of the second register;

   a second input end of the fourth point cloud multiplier, configured to receive a cosine function value corresponding to the rotation angle reached by the current rotation transformation;

   the first input end of the point cloud subtracter is connected with the output end of the first point cloud multiplier, the second input end of the point cloud subtracter is connected with the output end of the second point cloud multiplier, and the difference value output by the point cloud subtracter is set as the abscissa value of the rotating point cloud obtained after the point cloud is subjected to current rotation transformation; the point cloud subtracter is also used for transmitting the output difference value to the memory module through the interconnection bus and covering the abscissa of the point cloud stored in the memory module so as to update the abscissa result converted from the current rotation to the abscissa of the currently stored point cloud stored in the memory module;

   the first input end of the point cloud adder is connected with the output end of the third point cloud multiplier, the second input end of the point cloud adder is connected with the output end of the fourth point cloud multiplier, and the sum value output by the point cloud adder is set as the vertical coordinate value of the rotating point cloud obtained after the point cloud is subjected to current rotation transformation; the point cloud adder is also used for transmitting the output difference value to the memory module through the interconnection bus and covering the vertical coordinate of the point cloud stored in the memory module so as to update the vertical coordinate result converted from the current rotation to the vertical coordinate of the currently stored point cloud stored in the memory module;

   the rotation times are determined by a preset angle searching range of the searching window and a preset angle searching step length; each rotation transformation corresponds to one coordinate system transformation executed by the point cloud processing module and belongs to one matching processing of correlation scanning matching of the map.
9. 9. The correlation scan matching hardware circuit of any one of claims 1 to 8, wherein said state machine control module is of a finite state machine; the state machine control module is used for scheduling the working states of the memory module, the point cloud processing module and the TSDF value calculating module, so that when the point cloud processing module executes the coordinate system transformation and the rotation transformation once, the TSDF value calculating module executes the division operation once to obtain a TSDF value matched with a corresponding searched pose in the search window, and on the basis of updating the currently stored point cloud by using the result obtained by the rotation transformation, the point cloud processing module is controlled to continuously execute the coordinate system transformation and the TSDF value calculating module is controlled to execute the division operation once again until the TSDF value calculating module counts all the poses in the search window and calculates all the TSDF values matched with the poses in sequence;

   the searching step length is the number of grid searching which needs to be traversed when one pose is searched each time in the same searching window; the search window is a neighborhood grid region centered around a point cloud within the pre-expanded grid map.
10. 10. The hardware circuit of claim 8, wherein a bus interface module is disposed outside the hardware circuit, and the bus interface module includes a DMA controller module and a transmission bus;

    the DMA controller module is used for continuously transmitting data stored in the physical storage space of the discontinuous addresses in batches and reducing the triggering times of software interruption of the CPU;

    the transmission bus comprises a first bus and a second bus, the first bus is respectively in signal transceiving connection 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, the first bus is used for configuring data transmission parameters for the DMA controller module, and the first bus is also used for configuring parameters stored in the map size register, parameters stored in the map resolution register, expansion parameters required by the maximum detection radius reached by the currently stored point cloud, expansion parameters required by executing coordinate system transformation, expansion parameters required by executing rotation transformation, expansion parameters required by meeting the transmission requirement of bus bit width and search step length so as to realize mapping communication of the memory of the correlation scanning matching hardware circuit;

    the second bus is connected with the DMA controller module and is used for transmitting a truncation symbol distance function configured in advance by the CPU and a weight value matched with the truncation symbol distance function to the memory module;

    wherein the transmission bus is in compliance with AMBA protocol.
11. 11. A heterogeneous computing architecture, comprising a processor unit, a bus interface, and the correlation scan matching hardware circuit of 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 a parallel acceleration operation performed by the correlation scan matching hardware circuit, and the processor unit provides an operation parameter, a start signal, an interrupt signal, a reset signal, and a flag bit clearing signal for the correlation scan matching hardware circuit only 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 circuitry;

    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