劉?強，秦書臻，俞?波，劉少山

一種用于SLAM的嵌入式光束平差法加速器設(shè)計

劉?強1, 2，秦書臻1, 2，俞?波3，劉少山3

(1. 天津大學(xué)微電子學(xué)院，天津 300072；2. 天津市成像與感知微電子技術(shù)重點實驗室，天津 300072；3. 深圳普思英察科技有限公司，深圳 518000)

光束平差法是一種廣泛應(yīng)用于視覺SLAM系統(tǒng)后端的優(yōu)化算法，但在實際應(yīng)用中一直存在實時性差、能耗高的問題．本文在嵌入式FPGA平臺上實現(xiàn)了一款針對相機位姿已知的光束平差算法的加速器．該加速器為計算中最耗時的雅可比矩陣更新和成本函數(shù)計算模塊定制化硬件計算架構(gòu)．首先分析算法結(jié)構(gòu)去除冗余計算，再根據(jù)算法數(shù)據(jù)相關(guān)性設(shè)計流水線和并行處理單元，利用時分復(fù)用的方法將雅可比矩陣更新和成本函數(shù)計算在同一硬件上實現(xiàn)，同時在算法中引入猜測執(zhí)行進一步提高計算速度，既滿足了性能要求，又減小了硬件消耗．在Xilinx xc7z020上的實驗結(jié)果表明，本文設(shè)計的光束平差法加速器，相對于嵌入式ARM處理器有5.9倍的性能提升，同時節(jié)約能耗68.7%．

光束平差法；現(xiàn)場可編程門陣列；同步定位與地圖構(gòu)建

光束平差法(bundle adjustment，BA)在三維重建中同時優(yōu)化相機參數(shù)和三維結(jié)構(gòu)特征點位置，廣泛應(yīng)用于三維立體場景的重建[1]、同步定位和地圖構(gòu)建(simultaneous localization and mapping，SLAM)[2]. BA通過最小化重投影誤差來找到最優(yōu)的參數(shù)估計，即三維空間點位置和相機參數(shù)．該誤差被定義為三維點在圖片上的位置與根據(jù)輸入?yún)?shù)計算三維點在相機投影平面上位置差的L2范數(shù)[3]．

移動機器人可以利用SLAM在沒有預(yù)知地圖信息的條件下定位自身和對周圍環(huán)境建圖[4]．BA主要應(yīng)用于SLAM后端，對前端已經(jīng)建立的地圖進行優(yōu)化[5]．在SLAM發(fā)展的初期，其后端多使用擴展卡爾茲曼濾波算法[6]．之后，基于BA的SLAM系統(tǒng)[7-8]逐漸被開發(fā)出來，而且已經(jīng)證明基于BA的圖優(yōu)化方法在性能和精度上相較濾波算法更適合作為SLAM?后端[9]．

但是SLAM算法中密集的計算限制了SLAM在實際中的應(yīng)用．現(xiàn)場可編程門陣列(field programmable gate array，F(xiàn)PGA)擁有高并行和低功耗計算等優(yōu)勢，得到了研究人員的重視．2014年，蘇黎世聯(lián)邦理工學(xué)院設(shè)計了可以應(yīng)用于小型無人機的SLAM系統(tǒng)，其上搭載了4個攝像頭，使用FPGA對輸入圖像做預(yù)處理[10]．2017年帝國理工學(xué)院在FPGA上實現(xiàn)了SLAM中軌跡追蹤模塊的加速器，處理速度比嵌入式ARM處理器快了10倍以上[11]．同年，北京理工大學(xué)設(shè)計了一款基于FPGA的ORB特征提取器，幀率達(dá)到了67幀/s[12]．

目前SLAM算法在嵌入式端距實際應(yīng)用仍有一定的距離，其中BA是限制應(yīng)用的性能和功耗的瓶?頸[13]．針對BA的FPGA加速器研究處于起步階段. 2019年天津大學(xué)設(shè)計了可以加速典型BA計算中最耗時的舒爾消除的FPGA加速器[14]，對光束平差硬件加速器進行了探索．但是，在SLAM應(yīng)用設(shè)備已知自身的位姿[15]、僅需要對環(huán)境建圖時，或者應(yīng)用設(shè)備全球定位信號良好并搭載有高精度的實時動態(tài)設(shè)備可以獲取位姿時，BA計算中的舒爾消除可以省略，雅可比矩陣更新和成本函數(shù)計算成為了提高性能的瓶頸．

為了解決該問題，本文在嵌入式FPGA上設(shè)計了軟硬件協(xié)同的BA加速器，將已知相機位姿時BA計算中最消耗時間的雅可比矩陣更新和成本計算部分利用硬件進行加速．通過對投影函數(shù)及其雅可比矩陣算法進行分析，拆分了相機旋轉(zhuǎn)與重投影計算，減少了冗余計算．同時采用時分復(fù)用的方式在同一硬件下實現(xiàn)雅可比矩陣更新和成本函數(shù)計算，通過共享中間計算結(jié)果，進一步省略了不必要的計算，節(jié)省硬件消耗，降低功耗．另外，根據(jù)算法的數(shù)據(jù)依賴性設(shè)計相機旋轉(zhuǎn)與重投影計算的內(nèi)部流水線，同時可以復(fù)制重投影計算模塊以提升計算速度．該加速器設(shè)計可應(yīng)用于嵌入式SLAM，使設(shè)備能夠快速地完成建圖優(yōu)化任務(wù)．

1?算法基本原理

1.1?光束平差法

式中表示第個三維點是否被第張圖片觀測到.由于空間位置遮擋的關(guān)系，一張圖片并不能觀測到所有的三維點，當(dāng)?shù)趥€三維點被第張圖片觀測到時，否則．圖1為簡單的BA問題模型．

1.2?相機模型

1.3?Levenberg-Marquardt優(yōu)化算法

Levenberg-Marquardt(LM)算法是一種非線性最小二乘法，被廣泛用于尋找非線性函數(shù)的局部最小?值[18]．LM算法結(jié)合了最速下降法和高斯-牛頓法的優(yōu)點，使它可以從各種給定的初始條件中收斂．目前，LM已成為在視覺SLAM應(yīng)用中解決光束平差問題的標(biāo)準(zhǔn)算法[9]．

圖2?LM算法流程

2?硬件架構(gòu)設(shè)計

本文設(shè)計基于Xilinx Zynq系列處理器．該系列FPGA包含處理系統(tǒng)(processing system，PS)和可編程邏輯(programmable logic，PL)兩部分．PS部分為雙核ARM Cortex-A9處理器，執(zhí)行軟件程序．PL為可編程邏輯電路，可以實現(xiàn)硬件加速器．PS與PL之間通過AXI4總線進行數(shù)據(jù)交互．本文所設(shè)計的硬件加速器架構(gòu)如圖3所示．該架構(gòu)將雅可比矩陣更新和成本函數(shù)計算兩個耗時的部分在PL上采用硬件電路進行計算加速，其他部分在PS上使用軟件實現(xiàn)．軟件與硬件交互數(shù)據(jù)采用基于AXI4總線的直接內(nèi)存讀取單元(DMA)，硬件中使用FIFO做軟硬件數(shù)據(jù)交互的緩存．

圖3?硬件設(shè)計的架構(gòu)

圖4?旋轉(zhuǎn)矩陣處理器的架構(gòu)

圖5?重投影處理器的架構(gòu)

3?實驗結(jié)果與分析

本文使用搭載Xilinx xc7z020clg484-1 FPGA芯片的Zedboard開發(fā)板對提出的加速器設(shè)計進行評估．本文的設(shè)計與Google公司的開源最優(yōu)化庫Ceres-Solver[20]軟件進行比較．Ceres-Solver運行在兩個處理器平臺上：x86平臺使用帶有8GB內(nèi)存的Intel Core i5-8400，主頻為2.8GHz；ARM平臺使用Zynq-7000系列FPGA中內(nèi)置的雙核ARM Cortex-A9，帶有1GB內(nèi)存，主頻為667MHz．軟件實現(xiàn)使用雙精度浮點數(shù)，硬件設(shè)計使用單精度浮點數(shù)．RMP最大的時鐘頻率為50MHz，RPP最大的時鐘頻率為150MHz．評估的數(shù)據(jù)集選自華盛頓大學(xué)Bundle Adjustment in the Large(BAL)數(shù)據(jù)集[21]中圖片數(shù)量小于50的5個數(shù)據(jù)集，其所含有的照片數(shù)、三維點數(shù)目和觀測數(shù)見表1．該選用的數(shù)據(jù)集可以滿足小型SLAM中全局BA建圖和大型SLAM中局部建圖的要求[7]．本節(jié)主要評估了硬件設(shè)計的資源使用情況、性能和功耗．

表1?實驗所使用的數(shù)據(jù)集

Tab.1?Datasets used for the experiments

表2評估了本文硬件設(shè)計的資源消耗，前兩行分別列出了RMP模塊和RPP模塊的資源消耗情況．RMP模塊占用了FPGA芯片中17%的查找表(look up table，LUT)、11%的觸發(fā)器(flip-flop，F(xiàn)F)、0.5個塊隨機存取存儲器(block-RAM，BRAM)和12個數(shù)字信號處理單元(digital signal processor，DSP)．RPP模塊占用了13%的LUT、9%的FF、15個BRAM和30個DSP．因為計算所需要暫存的數(shù)據(jù)較少，RMP使用的是利用LUT組成的分布式RAM，同時因為近似串行的設(shè)計也減少了片上DSP的用量．考慮到FPGA片上資源和布局布線的問題，最終在FPGA上實現(xiàn)了5個RPP，相比僅使用一個RPP可以縮短近80%的處理時間．

表3評估了雅可比矩陣更新和成本函數(shù)計算模塊在3個平臺上運行各數(shù)據(jù)集的執(zhí)行時間，表格括號中為成本函數(shù)的計算時間和加速比．利用Intelx86處理器處理5組數(shù)據(jù)集，更新雅可比矩陣平均要消耗35.497ms，成本函數(shù)計算平均需要5.971ms，而本文設(shè)計的硬件加速器平均使用4.199ms即可完成上述的兩項工作．相比x86平臺，本文設(shè)計在雅可比矩陣更新和成本函數(shù)計算分別有8.453倍和1.422倍的加速效果．對比低功耗的ARM平臺，本文的加速器在雅可比矩陣更新上實現(xiàn)248.608倍的加速效果，在成本函數(shù)計算上也有22.570倍加速．

表2?硬件設(shè)計的資源消耗

Tab.2?Hardware resource utilization

表3?軟硬件實現(xiàn)在雅可比矩陣更新和成本函數(shù)計算上的性能比較

Tab.3?Performance comparison of Jacobian Update and Cost Calculation between hardware and software implementations

此外，本文還評估了整體光束平差法在3個計算平臺上的性能和所消耗的能量，結(jié)果如表4所示．Intel CPU的功率為65W，Xilinx功耗評估器分別給出了ARM平臺和FPGA平臺的功率，分別為1.5W和2.8W．在Intel x86，ARM和FPGA平臺上處理5個數(shù)據(jù)集，單次BA迭代的平均執(zhí)行時間分別為51.029ms、1363.232ms和228.646ms．FPGA平臺的性能比嵌入式ARM處理器有5.962倍的提升，可以滿足實時建圖的要求．由于算法中還存在部分計算使用ARM處理器執(zhí)行，在光束平差法的執(zhí)行時間上，本文設(shè)計比x86平臺要長．3種平臺上Intel處理器實現(xiàn)的速度最快，但高功率不適合嵌入式終端使用．本文實現(xiàn)的FPGA設(shè)計相比Intel x86處理器和ARM處理器分別可以節(jié)省80.7%和68.7%的能量消耗，在高性能的同時也節(jié)約了能量消耗，使得該加速器在嵌入式設(shè)備上的續(xù)航更長．文獻(xiàn)[14]中的BA硬件加速器，由于針對的是典型BA算法并且加速了舒爾消除步驟，在這5個數(shù)據(jù)集上的平均迭代時間為1.29s．本文加速器針對SLAM應(yīng)用設(shè)備已知自身位姿的情況，優(yōu)化了計算，提升了性能．SLAM開發(fā)者可以根據(jù)具體的應(yīng)用場景選擇相應(yīng)的BA加速方案．

表4?光束平差法上的性能與功耗比較

Tab.4?Performance and energy consumption comparison of bundle adjustment

4?結(jié)?語

本文針對SLAM后端已知相機位姿建圖問題提出了一種軟件硬件結(jié)合的BA加速器，通過時分復(fù)用、猜測執(zhí)行、并行計算等技術(shù)，實現(xiàn)了高能效的計算．本文的硬件加速器可以在資源、功率受限的嵌入式設(shè)備上在已知相機位姿的條件下高效地完成實時建圖任務(wù)，實現(xiàn)機器人計算和視覺重建地本地化．

［1］ 景子君. 運動法三維重建的研究與實現(xiàn)[D]. 合肥：中國科學(xué)技術(shù)大學(xué)，2018.

Jing Zijun. Study and Reality of 3D Reconstruction for SFM[D]. Hefei：University of Science and Technology of China，2018(in Chinese).

［2］ 姚二亮，張合新，張國良，等. 基于Vision-IMU的機器人同時定位與地圖創(chuàng)建算法[J]. 儀器儀表學(xué)報，2018，39(4)：230-238.

Yao Erliang，Zhang Hexin，Zhang Guoliang，et al. Robot simultaneous localization and mapping algorithm based on vision and IMU[J]. Chinese Journal of Scientific Instrument，2018，39(4)：230-238(in Chinese).

［3］ Philip H S T，Andrew Z. Vision Algorithms：Theory and Practice[M]. Berlin：Springer Berlin Heidelberg，1999.

［4］ Davison A J，Reid I D，Molton N D，et al. MonoSLAM：Real-time single camera SLAM[J]. IEEE Trans Pattern Anal Mach Intell，2007，29(6)：1052-1067.

［5］ 權(quán)美香，樸松昊，李?國. 視覺SLAM綜述[J]. 智能系統(tǒng)學(xué)報，2016，11(6)：768-776.

Quan Meixiang，Piao Songhao，Li Guo. An overview of visual SLAM[J]. CAAI Transactions on Intelligent Systems，2016，11(6)：768-776(in Chinese).

［6］ Cadena C，Carlone L，Carrillo H，et al. Past，present，and future of simultaneous localization and mapping：Toward the robust-perception age[J]. IEEE Transactions on Robotics，2016，32(6)：1309-1332.

［7］ Mur-Artal R，Montiel J M M，Tardos J D. ORB-SLAM：A versatile and accurate monocular SLAM system[J]. IEEE Transactions on Robotics，2015，31(5)：1147-1163.

［8］ 林輝燦，呂?強，王國勝，等. 魯棒的非線性優(yōu)化的立體視覺-慣導(dǎo)SLAM[J]. 機器人，2018，40(6)：911-920.

Lin Huican，Lü Qiang，Wang Guosheng，et al. Robust stereo visual-inertial SLAM using nonlinear optimization[J]. Robot，2018，40(6)：911-920(in Chinese).

［9］ Strasdat H，Montiel J M M，Davison A J. Visual SLAM：Why filter?[J]. Image and Vision Computing，2012，30(2)：65-77.

［10］ Nikolic J，Rehder J，Burri M，et al. A synchronized visual-inertial sensor system with FPGA pre-processing for accurate real-time SLAM[C]// Proceedings of the 2014 IEEE International Conference on Robotics and Automation(ICRA). Piscataway，USA，2014：431-437.

［11］ Boikos K，Bouganis C S. A high-performance system-on-chip architecture for direct tracking for SLAM[C]// Proceedings of the 2017 27th International Conference on Field Programmable Logic and Applications(FPL). Piscataway，USA，2017：1-7.

［12］ Fang W，Zhang Y，Yu B，et al. FPGA-based ORB feature extraction for real-time visual SLAM[C]. Proceedings of the 2017 International Conference on Field Programmable Technology(ICFPT). Piscataway，USA，2017：275-278.

［13］ 劉?康. 大尺度視覺SLAM的光束平差算法研究[D]. 北京：北京郵電大學(xué)，2018.

Liu Kang. Research on Bundle Adjustment for Visual SLAM Under Large-Scales Scence[D]. Beijing：Beijing University of Posts and Telecommunications，2018(in Chinese).

［14］ Qin S，Liu Q，Yu B，et al. π-BA：Bundle adjustment acceleration on embedded FPGAs with co-observation optimization[C]//2019 IEEE 27th Annual International Symposium on Field-Programmable Custom Computing Machines(FCCM). Piscataway，USA，2019：100-108.

［15］ Konolige K. Large-scale map-making[C]//Proceedings of the 19th National Conference on Artificial Intelligence. Palo Alto，USA，2004：457-463.

［16］ Fioraio N，Di Stefano L. Joint detection，tracking and mapping by semantic bundle adjustment[C]// Proceedings of the IEEE Conference on Computer Vision and Pattern Recognition. Piscataway，USA，2013：1538-1545.

［17］ 解則曉，周作琪. 基于運動恢復(fù)結(jié)構(gòu)的空間點定位方法[J]. 激光與光電子學(xué)進展，2018，55(8)：370-377. Xie Zexiao，Zhou Zuoqi. Method of space point positioning based on structure-from-motion[J]. Laser & Optoelectronics Progress，2018，55(8)：370-377(in Chinese).

［18］ Madsen K，Nielsen H B，Tingleff O. Methods for Non-Linear Least Squares Problems[EB/OL]. http：//www2. imm.dtu.dk/pubdb/views/edoc_download.php/3215/pdf/imm3215.pdf，2004-07-14.

［19］ Lourakis M L A，ArgyrosAA. Is Levenberg-Marquardt the most efficient optimization algorithm for implementing bundle adjustment?[C]//Tenth IEEE International Conference on Computer Vision (ICCV'05) Volume 1. Piscataway，USA，2005，2：1526-1531.

［20］ Sameer Agarwal. Ceres Solver[EB/OL]. http：//ceres-solver.org，2018-06-30.

［21］ Agarwal S，Snavely N，Seitz S M，et al. Bundle adjustment in the large[C]//European Conference on Computer Vision. Berlin，Germany，2010：29-42.

An Embedded Bundle Adjustment Accelerator Design for SLAM

Liu Qiang1, 2，Qin Shuzhen1, 2，Yu Bo3，Liu Shaoshan3

(1. School of Microelectronics，Tianjin University，Tianjin 300072，China；2. Tianjin Key Laboratory of Imaging and Sensing Microelectronic Technology，Tianjin 300072，China；3. Shenzhen PerceptIn Technology Co.，Ltd.，Shenzhen 518000，China)

Bundle adjustment(BA)is an optimization algorithm widely used in the backend of visual SLAM systems，but it exhibits the problems of low real-time performance and high energy consumption in practice. This paper designs an embedded FPGA accelerator for BA with a known camera pose. The accelerator customizes hardware computing architectures for Jacobian matrix update and cost function calculation which are the most time-consuming parts of the BA. The bundle adjustment algorithm is first analyzed to remove redundant calculations，and then the pipelined and parallel processing unit is designed based on the algorithm’s data dependence. Using time division multiplexing，Jacobian matrix update and cost function calculation are implemented on the same hardware，and the speculative execution is incorporated into the algorithm to further improve computation speed. By doing so，the hardware resources are saved and the performance requirements are satisfied. The experimental results on Xilinx xc7z020 indicate that the accelerator has a 5.9 fold performance improvement，compared with the embedded ARM processor. At the same time，the accelerator can save 68.7% of energy.

bundle adjustment；field programmable gate array；simultaneous localization and mapping

TP368

A

0493-2137(2020)12-1281-07

10.11784/tdxbz201909072

2019-09-30；

2020-05-11.

劉?強（1978—??），男，博士，教授.

劉?強，qiangliu@tju.edu.cn.

國家自然科學(xué)基金資助項目(61974102).

Supported by the National Natural Science Foundation of China(No. 61974102).

(責(zé)任編輯：王曉燕)