Hardware-Software Co-Design of Matrix-Solving for Non-Linear Optimization in SLAM Systems

Simultaneous Localization and Mapping (SLAM) is one of the most important techniques for autonomous robots that enables the robot aware of its current position and the surrounding environment. There is a significant improvement in the accuracy with the advancement in SLAM algorithms. However, the computation complexity increases accordingly and the embedded processors of autonomous robots struggle to support heavy calculation. Matrix-solving contributes a major portion of calculation time and considering sub-tasks such as bundle adjustment takes over 40% of total time. Therefore, it is significant to optimize the calculations required for matrix-solving. However, previous works for matrix-solving accelerators are generalized and the specific matrix form in SLAM problems is not fully considered. This work concentrates on the dedicated software and hardware codesign of the matrix-solving task in SLAM systems and provides three solutions for different scales of matrix-solving problems in SLAM. The proposed FSFI-Cholesky and FI-Iterative method have achieved up to 120.2x speed improvement over the non-optimized Cholesky algorithm. Moreover, this work also reduces the execution time by more than 7.0x compared to the state-of-the-art design with fewer DSPs used for both dense and sparse matrices.