Research On CPU-FPGA Parallelization Of Lidar SLAM Algorithm For Mobile Robot

In recent years,the market for mobile robots is growing.In order to reduce the cost and improve the precision,the service robot,represented by the sweeping robot,gradually adopts the scheme of special chip.The algorithm of lidar SLAM(Simultaneous Localization And Mapping)is often complicated with a large amount of computation,and the general processor can not meet the real-time requirements.In order to meet the requirements of accuracy and real-time performance in the practical application of lidar SLAM,this paper uses CPU-FPGA(Central Processing Unit Field-Programmable Gate Array)heterogeneous processor as the computing platform,gives full play to the characteristics of software flexibility and hardware parallel performance,and puts forward the localization and relocation implementation scheme combining software and hardware.Lidar SLAM includes sensor data acquisition,data preprocessing,real-time scan matching and mapping,and global loop detection and optimization.Correspondingly,we select two tasks: real-time CSM(Correlation Scan Matching)and branch and bound.In the real-time CSM,the current lidar point is corresponding to the local map to obtain more accurate pose in real time by means of the initial pose provided by the encoder and the IMU(Inertial measurement unit).The branching and bound further accelerates the relocation process when the initial pose is not accurate and the pose needs to be searched in a wide range.The research in this paper is the most time-consuming part of lidar SLAM,which is of great significance to the design of special chip for robot.In order to solve the real-time positioning problem of mobile robot,the real-time CSM algorithm is implemented on CPU-FPGA heterogeneous processor for the first time in this paper,and the pipeline parallel algorithm module is designed and implemented.Considering the characteristics of probabilistic raster map and TSDF(Truncated Signed Distance Function)map,the algorithm module can accelerate for both maps.Aiming at the algorithm module's Linux application,the DMA(Direct Memory Access)module and the algorithm hardware module's Linux driver are designed,and the driver is written to test the algorithm performance.Experimental results show that the system can process 16 m lidar at89 fps on Xilinx Zynq7000.Besides,the absolute error of rotation does not exceed the physical angular resolution of lidar,and the absolute error of translation does not exceed the physical resolution of grid resolution.Based on the above research,this paper further studies the robot relocation problem when the search window expands.In this paper,the branching,pruning and sorting of branch and bound algorithm are handled flexibly by CPU.The high parallelism of the hardware algorithm module is brought into play,and the upper bound value of the branch and bound is calculated with the help of FGPA.Considering the limitations of FPGA hardware resources,a small part of the upper bound computation is allocated to the CPU.In order to reduce the consumption of CPU hardware resources by interrupts,two hardware modes of polling and interrupting are designed in this paper,and the branch-and-bound algorithm is further accelerated by 50%.The experimental results show that the algorithm's frame rate reaches 19.4 fps under the 9.6 m 9.6 m search window,and the system can achieve throughput rate of 13 GOPS with a working frequency of 133.33 MHz.