面向自主移动机器人的可重构视觉惯性里程计协处理器设计

随着自主移动机器人的广泛应用，通过相机和惯性测量单元的融合数据以实现定位功能的视觉惯性里程计系统发展迅速，但仍受到算法复杂度高、专用加速器开发周期长和自主移动机器人平台的低功率供应能力等因素限制。

本文基于惯性传感器测量得到的加速度和角速度数据，结合单目灰度图像的FAST特征，使用扩展卡尔曼滤波器实现了自主移动机器人的位姿估计。为此设计了一种可重构的视觉惯性里程计协处理器，可支持不同的基于滤波优化的视觉惯性里程计算法，具备应用场景灵活的优势。另外，本设计具有低功耗的优势，适用于自主移动机器人这一轻量级平台，且拥有低资源消耗和高性能的特性。实验结果表明，在最具权威性的数据集EuRoC的11个场景中，所提出的可重构视觉惯性里程计协处理器对位姿估计的精度损失最大为0.03%。片上内存使用量为70KB，至少比最新的工作小10倍，LUTs、FFs和DSPs等硬件资源消耗也优于现有工作。在最高160MHz的工作频率下，本设计在UltraScale+ XCVU440 FPGA平台的功耗约为0.638W。在28nm CMOS工艺的逻辑综合结果中，使用约50%的逻辑单元利用率，本视觉惯性里程计协处理器的core面积仅为3mm²。

[1] KIM A, EUSTICE R M. Real-Time Visual SLAM for Autonomous Underwater Hull Inspection Using Visual Saliency[J]. IEEE Transactions on Robotics, 2013, 29(3): 719-733.
[2] ZHOU H Z, ZOU D P, PEI L, et al. StructSLAM: Visual SLAM With Building Structure Lines[J]. IEEE Transactions on Vehicular Technology, 2015, 64(4): 1364-1375.
[3] NGUYEN D D, ELOUARDI A, FLOREZ S A R, et al. HOOFR SLAM System: An Embedded Vision SLAM Algorithm and Its Hardware-Software Mapping-Based Intelligent Vehicles Applications[J]. IEEE Transactions on Intelligent Transportation Systems, 2019, 20(11): 4103-4118.
[4] 权美香,朴松昊,李国. 视觉SLAM综述[J]. 智能系统学报,2016,11(6):768-776.
[5] MEIRELES M, LOURENCO R, DIAS A, et al. Real Time Visual SLAM for underwater robotic inspection; proceedings of the Oceans Conference, St Johns, CANADA, F Sep 14-19, 2014 [C]. 2014.
[6] WOOD R J, FINIO B, KARPELSON M, et al. Progress on 'pico' air vehicles[J]. International Journal of Robotics Research, 2012, 31(11): 1292-1302.
[7] MA K Y, CHIRARATTANANON P, FULLER S B, et al. Controlled Flight of a Biologically Inspired, Insect-Scale Robot[J]. Science, 2013, 340(6132): 603-607.
[8] SILVEIRA G, MALIS E, RIVES P. An Efficient Direct Approach to Visual SLAM[J]. IEEE Transactions on Robotics, 2008, 24(5): 969-979.
[9] NEWCOMBE R A, LOVEGROVE S J, DAVISON A J, et al. DTAM: Dense Tracking and Mapping in Real-Time; proceedings of the IEEE International Conference on Computer Vision (ICCV), Barcelona, SPAIN, F Nov 06-13, 2011 [C]. 2011.
[10] ENGEL J, STUCKLER J, CREMERS D, et al. Large-Scale Direct SLAM with Stereo Cameras; proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Hamburg, GERMANY, F Sep 28-Oct 02, 2015 [C]. 2015.
[11] FORSTER C, PIZZOLI M, SCARAMUZZA D, et al. SVO: Fast Semi-Direct Monocular Visual Odometry; proceedings of the IEEE International Conference on Robotics and Automation (ICRA), Hong Kong, PEOPLES R CHINA, F May 31-Jun 07, 2014 [C]. 2014.
[12] ENGEL J, KOLTUN V, CREMERS D. Direct Sparse Odometry[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2018, 40(3): 611-625.
[13] SUN J M, WANG Y Q, SHEN Y Y, et al. Fully Scaled Monocular Direct Sparse Odometry with A Distance Constraint; proceedings of the 5th International Conference on Control, Automation and Robotics (ICCAR), Beijing, PEOPLES R CHINA, F Apr 19-22, 2019 [C]. 2019.
[14] WANG X G, WU F C, WANG Z H, et al. Harris Feature Vector Descriptor (HFVD); proceedings of the 19th International Conference on Pattern Recognition (ICPR 2008), Tampa, FL, F Dec 08-11, 2008 [C]. 2008.
[15] LOWE D G. Distinctive image features from scale-invariant keypoints[J]. International Journal of Computer Vision, 2004, 60(2): 91-110.
[16] MEHROTRA R, NICHANI S, RANGANATHAN N. CORNER DETECTION[J]. Pattern Recognition, 1990, 23(11): 1223-1233.
[17] BAY H, ESS A, TUYTELAARS T, et al. Speeded-Up Robust Features (SURF)[J]. Computer Vision and Image Understanding, 2008, 110(3): 346-359.
[18] ROSTEN E, PORTER R, DRUMMOND T. Faster and Better: A Machine Learning Approach to Corner Detection[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2010, 32(1): 105-119.
[19] RUBLEE E, RABAUD V, KONOLIGE K, et al. ORB: an efficient alternative to SIFT or SURF; proceedings of the IEEE International Conference on Computer Vision (ICCV), Barcelona, SPAIN, F Nov 06-13, 2011 [C]. 2011.
[20] CASTELLANOS J A, MONTIEL J M M, NEIRA J, et al. The SPmap: A probabilistic framework for simultaneous localization and map building[J]. IEEE Transactions on Robotics and Automation, 1999, 15(5): 948-952.
[21] HUANG G P, MOURIKIS A I, ROUMELIOTIS S I, et al. Analysis and improvement of the consistency of extended Kalman filter based SLAM; proceedings of the IEEE International Conference on Robotics and Automation, Pasadena, CA, F May 19-23, 2008 [C]. 2008.
[22] THRUN S, LIU Y F, KOLLER D, et al. Simultaneous localization and mapping with sparse extended information filters[J]. International Journal of Robotics Research, 2004, 23(7-8): 693-716.
[23] EUSTICE R M, SINGH H, LEONARD J J. Exactly sparse delayed-state filters for view-based SLAM[J]. IEEE Transactions on Robotics, 2006, 22(6): 1100-1114.
[24] MONTEMERLO M, THRUN S, KOLLER D, et al. FastSLAM: A factored solution to the simultaneous localization and mapping problem; proceedings of the 18th National Conference on Artificial Intelligence/14th Conference on Innovative Applications of Artificial Intelligence, Edmonton, Canada, F Jul 28-Aug 01, 2002 [C]. 2002.
[25] HAHNEL D, BURGARD W, FOX D, et al. An efficient FastSLAM algorithm for generating maps of large-scale cyclic environments from raw laser range measurements; proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems, Las Vegas, Nv, F Oct 27-31, 2003 [C]. 2003.
[26] GRISETTI G, STACHNISS C, BURGARD W. Improved techniques for grid mapping with Rao-Blackwellized particle filters[J]. IEEE Transactions on Robotics, 2007, 23(1): 34-46.
[27] DI K C, ZHAO Q, WAN W H, et al. RGB-D SLAM Based on Extended Bundle Adjustment with 2D and 3D Information[J]. Sensors, 2016, 16(8).
[28] ALISMAIL H, BROWNING B, LUCEY S. Photometric Bundle Adjustment for Vision-Based SLAM; proceedings of the 13th Asian Conference on Computer Vision (ACCV), Taipei, TAIWAN, F Nov 20-24, 2016 [C]. 2017.
[29] LIU H M, CHEN M Y, ZHANG G F, et al. ICE-BA: Incremental, Consistent and Efficient Bundle Adjustment for Visual-Inertial SLAM; proceedings of the 31st IEEE/CVF Conference on Computer Vision and Pattern Recognition (CVPR), Salt Lake City, UT, F Jun 18-23, 2018 [C]. 2018.
[30] DAVISON A J, REID I D, MOLTON N D, et al. MonoSLAM: Real-time single camera SLAM[J]. IEEE Transactions on Pattern Analysis and Machine Intelligence, 2007, 29(6): 1052-1067.
[31] KLEIN G, MURRAY D. Parallel Tracking and Mapping on a Camera Phone; proceedings of the 8th IEEE International Symposium on Mixed and Augmented Reality, Orlando, FL, F Oct 19-22, 2009 [C]. 2009.
[32] BLOESCH M, OMANI S, HUTTER M, et al. Robust Visual Inertial Odometry Using a Direct EKF-Based Approach; proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Hamburg, GERMANY, F Sep 28-Oct 02, 2015 [C]. 2015.
[33] LEUTENEGGER S, LYNEN S, BOSSE M, et al. Keyframe-based visual-inertial odometry using nonlinear optimization[J]. International Journal of Robotics Research, 2015, 34(3): 314-334.
[34] QIN T, LI P L, SHEN S J. VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator[J]. IEEE Transactions on Robotics, 2018, 34(4): 1004-1020.
[35] JIANG C, GENG Z X, WEI X F, et al. SIFT implementation based on GPU; proceedings of the 5th International Symposium on Photoelectronic Detection and Imaging (ISPDI) - Optical Storage and Display Technology, Beijing, PEOPLES R CHINA, F Jun 25-27, 2013 [C]. 2013.
[36] CORNELIS N, VAN GOOL L, IEEE. Fast Scale Invariant Feature Detection and Matching on Programmable Graphics Hardware; proceedings of the IEEE Conference on Computer Vision and Pattern Recognition, Anchorage, AK, F Jun 23-28, 2008 [C]. 2008.
[37] YUM J, LEE C H, KIM J S, et al. A Novel Hardware Architecture With Reduced Internal Memory for Real-Time Extraction of SIFT in an HD Video[J]. IEEE Transactions on Circuits and Systems for Video Technology, 2016, 26(10): 1943-1954.
[38] WILSON C, ZICARI P, CRACIUN S, et al. A Power-Efficient Real-Time Architecture for SURF Feature Extraction; proceedings of the 2014 International Conference on Reconfigurable Computing and FAGAs, Natl I Astrophysics Optics Elect (INAOE), Cancun, MEXICO, F Dec 08-10, 2014 [C]. 2014.
[39] ULUSEL O, PICARDO C, HARRIS C B, et al. Hardware Acceleration of Feature Detection and Description Algorithms on Low-Power Embedded Platforms; proceedings of the 26th International Conference on Field-Programmable Logic and Applications (FPL), Ecole Polytechnique Federale de Lausanne, Lausanne, SWITZERLAND, F Aug 29-Sep 02, 2016 [C]. 2016.
[40] TERTEI D T, PIAT J, DEVY M. FPGA design of EKF block accelerator for 3D visual SLAM[J]. Computers & Electrical Engineering, 2016, 55: 123-137.
[41] WANG J P, ZHAN Y, WANG Z X, et al. A Reconfigurable Matrix Multiplication Coprocessor with High Area and Energy Efficiency for Visual Intelligent and Autonomous Mobile Robots; proceedings of the 17th IEEE Asian Solid-State Circuits Conference (A-SSCC) - Integrated Circuits and Systems for the Connection of Intelligent Things, Busan, SOUTH KOREA, F Nov 07-10, 2021 [C]. 2021.
[42] LIU R Z, YANG J L, CHEN Y R, et al. eSLAM: An Energy-Efficient Accelerator for Real-Time ORB-SLAM on FPGA Platform; proceedings of the 56th ACM/EDAC/IEEE Design Automation Conference (DAC), Las Vegas, NV, F Jun 02-06, 2019 [C]. 2019.
[43] LI Z Y, CHEN Y, GONG L Y, et al. An 879GOPS 243mW 80fps VGA Fully Visual CNN-SLAM Processor for Wide-Range Autonomous Exploration; proceedings of the IEEE International Solid-State Circuits Conference (ISSCC), San Francisco, CA, F Feb 17-21, 2019 [C]. 2019.
[44] SULEIMAN A, ZHANG Z D, CARLONE L, et al. Navion: A 2-mW Fully Integrated Real-Time Visual-Inertial Odometry Accelerator for Autonomous Navigation of Nano Drones[J]. IEEE Journal of Solid-State Circuits, 2019, 54(4): 1106-1119.
[45] ZHANG Z D, SULEIMAN A, CARLONE L, et al. Visual-Inertial Odometry on Chip: An Algorithm-and-Hardware Co-design Approach; proceedings of the 13th Conference on Robotics - Science and Systems, Massachusetts Inst Technol, Cambridge, MA, F Jul 12-16, 2017 [C]. 2017.
[46] WANG C, LIU Y K, ZUO K D, et al. ac(2)SLAM: FPGA Accelerated High-Accuracy SLAM with Heapsort and Parallel Keypoint Extractor; proceedings of the 20th International Conference on Field-Programmable Technology (ICFPT), Electr Network, F Dec 06-10, 2021 [C]. 2021.
[47] LUPTON T, SUKKARIEH S. Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions[J]. IEEE Transactions on Robotics, 2012, 28(1): 61-76.
[48] FORSTER C, CARLONE L, DELLAERT F, et al. IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation; proceedings of the 11th Conference on Robotics - Science and Systems, Sapienza Univ Rome, Rome, ITALY, F Jul 13-17, 2015 [C]. 2015.
[49] GAUTSCHI M, SCHAFFNER M, GURKAYNAK F K, et al. An Extended Shared Logarithmic Unit for Nonlinear Function Kernel Acceleration in a 65-nm CMOS Multicore Cluster[J]. IEEE Journal of Solid-State Circuits, 2017, 52(1): 98-112.
[50] CAO N Y, CHANG M Y, RAYCHOWDHURY A. A 65-nm 8-to-3-b 1.0-0.36-V 9.1-1.1-TOPS/W Hybrid-Digital-Mixed-Signal Computing Platform for Accelerating Swarm Robotics[J]. IEEE Journal of Solid-State Circuits, 2020, 55(1): 49-59.
[51] KIM Y, SHIN D, LEE J, et al. A 0.55 V 1.1 mW Artificial Intelligence Processor With On-Chip PVT Compensation for Autonomous Mobile Robots[J]. IEEE Transactions on Circuits and Systems I-Regular Papers, 2018, 65(2): 567-580.
[52] LIU Q, WAN Z S, YU B, et al. An Energy-Efficient and Runtime-Reconfigurable FPGA-Based Accelerator for Robotic Localization Systems; proceedings of the IEEE Custom Integrated Circuits Conference (CICC), Newport Beach, CA, USA, F Apr 24-27, 2022, [C]. 2022.