一種用于SLAM 的IMU 狀態優化加速器設計

劉強，劉威壯，俞波，劉少山

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

同步定 位與建 圖（simultaneous localization and mapping，SLAM）是當前實現移動機器人自主定位與環境感知的關鍵技術[1]。為了感知復雜的環境，SLAM 系統往往需要融合內感受型傳感器（如IMU）和外感受型傳感器（如相機、GPS）[2]。慣性測量單位（inertial measurement unit，IMU）是加速度計和陀螺儀的組合，可獲得物體自身的加速度值和角速度值，從而用來估計移動物體的位置、方向和速度，并且具有不受外界環境影響的特點。SLAM 系統可以在隧道（GPS 信息丟失）和地下黑暗（視覺信息消失）環境中依賴IMU 信息進行定位[3]。

SLAM 系統包括2 個主要組件：前端和后端。前端將傳感器數據抽象為適合優化的模型，后端對前端產生的抽象數據進行推理，優化移動的軌跡[4]。后端又分為濾波和非線性優化2 種實現方法。與濾波方法相比，非線性優化方法可以實現更精確的狀態估計，已成為目前研究的重點[5]。非線性優化方法使用由系統狀態得出的理論值和觀測數據之間的誤差項來構建成本函數，通過最小化該成本函數的值來優化系統狀態，通常使用的算法有高斯牛頓（Gauss-Newton）算法和列文伯格-馬夸爾特（Levenberg-Marquardt，LM）算法等。

雖然基于非線性優化的后端方法可以提升系統的定位精度和魯棒性，但是高計算量和數據復雜性限制了其在功率受限、實時性要求高情況下的應用?，F 場 可 編 程 門 陣 列（field programmable gate array，FPGA）具有功耗低、適合大規模并行計算及可重配置的特點。近年來，研究者們提出了多個基于FPGA 的SLAM 后端加速器。2020 年，天津大學在嵌入式FPGA-SoC 上實現了軟硬件協同的SLAM后端硬件加速器[6]，同時提出了共視優化技術和硬件友好的微分方法。同年，上海交通大學提出了基于FPGA 的全硬件SLAM 后端加速器[7]。佐治亞理工學院提出將SLAM 中稀疏矩陣運算轉換為固定大小的密集矩陣運算的方法，并在FPGA 上實現了低功耗的SLAM 后端加速器[8]。然而，這些關于SLAM 后端加速器的研究都僅考慮了視覺信息，沒有涉及對IMU 觀測信息的加速處理。隨著IMU在SLAM 中的應用越來越廣泛，研究IMU 信息的加速處理變得很有必要。

本文在FPGA 上設計了用于IMU 狀態優化的SLAM 后端加速器。加速器采用LM 算法，為算法中的雅可比矩陣計算、海森矩陣計算和求解線性方程組等步驟定制硬件計算架構，利用算法流程和數據的特點實現復用。同時，充分利用IMU 狀態優化問題中特有的稀疏特性，簡化矩陣計算和存儲。對于計算量最大的求解線性方程組的硬件，采用電路規?？膳渲玫脑O計，實現電路性能、資源消耗和功耗的折中。

1 算法原理

1.1 優化問題描述

IMU 狀態由位姿、速度和偏置3 個參數定義。在 時 刻i，IMU 狀 態 可 表 示 為xi=(pi,qi,vi,bai,bgi)。其中，(pi,qi)表 示位置和姿態，pi∈R3為平移向量，表示當前IMU 相對于坐標原點的位置，四元數qi表示IMU 的姿態，即相對于參考坐標系的旋轉；vi∈R3為 IMU 三 個 軸 上 的 速 度；bai,bgi∈R3分 別 為IMU 三個軸的加速度偏置和繞三個軸旋轉的角速度偏置。

IMU 的測量頻率通常為100～3 000 Hz。在基于優化的方法中，如果為每個IMU 測量定義一個新狀態，系統將由于計算量過大而無法處理[9]。因此，需要減少系統中優化的狀態量。圖1 為IMU 積分示意圖。通過對一段時間 ?t內的慣性測量進行積分，得出ti和ti+?t之間IMU 測量的積分值，即將?t內的多個慣性測量值合并為一個相對運動約束，從而定義系統在ti和ti+?t時刻的狀態。IMU 測量的積分由SLAM 前端完成，作為后端優化的輸入。IMU 測量的積分值通常使用歐拉積分的方法在離散時間上積分得出[10]。

圖1 IMU 積分示意圖Fig.1 Schematic diagram of IMU integration

將時間段ti～tj上的IMU 測量積分值定義為yi,j，yi,j=(?p,?q,?v)分別表示平移、旋轉和速度的積分值。同時，利用IMU 在ti和tj時刻的狀態xi和xj，可以計算出積分的理論值y?i,j。但由于測量噪聲的存在，y?i,j與yi,j之間存在誤差，誤差的值可表示為ri,j(xi,xj,yi,j)=y?i,j?yi,j。式(1)展示了IMU 誤差的具體計算方法，計算結果是15 維的向量。

式中：“?”表示四元數的乘積運算；Ri為四元數qi的旋轉矩陣形式；G為重力加速度。

1.2 優化問題求解

式(2)是一個復雜的非線性最小二乘問題，通常采用迭代方法求解該最小二乘問題[11]，即通過不斷更新優化的變量使F(X)的值下降。由于LM 算法實現容易，收斂快速[12]，廣泛應用在SLAM 后端中求解該問題。

算法1 描述了LM 算法流程。該算法輸入的是待優化的初始狀態X0和積分值Y0。算法輸出的是使成本函數(2)最小的狀態變量X+。在算法描述中，為了書寫的簡潔，使用r(X,Y0)表示系統中所有的誤差項，即N個待優化的狀態和N?1 個積分值形成的N?1 個誤差項。同樣，r′(X,Y0)表示所有誤差項對系統整體狀態的導數。LM 算法是迭代算法，迭代停止的條件為：①達到最大迭代次數kmax；②算法1 中 第6 行 梯 度g的 無 窮 范 數 //g//∞<ε1；③第9 行中X的更新變化小于 ε2。kmax和 ε1、ε2及初始信任域半徑 μ0均是算法的經驗參數。

算法首先計算初始狀態的誤差和誤差對狀態變量的導數即雅可比矩陣J。在每次的迭代中，根據當前的雅可比矩陣J和誤差 ?計算海森矩陣H和梯度g。海森矩陣原為誤差對狀態變量的二階導數，LM 算法中使用雅可比矩陣的轉置JT和J的乘積來近似。然后通過求解線性方程(H+μI)δX=g來計算X的更新步長 δX，該線性方程稱為最小二乘的正規方程。利用正規方程的解更新狀態得到Xnew，計算Xnew對應的成本函數，并判斷是否滿足成本函數減小，若不滿足則需要擴大信任域半徑重新尋找迭代步長 δX，若滿足則將狀態更新為Xnew，并重新計算對應的誤差和雅可比矩陣進行下一次迭代。

求解正規方程是算法中計算復雜度最高的步驟，正規方程中系數矩陣H+μI是對稱正定矩陣[13]，為了快速穩定地求解該方程，通常使用喬可斯基分解算法進行求解。喬可斯基分解算法將對稱正定矩陣S分解成下三角矩陣L和上三角矩陣LT，使得S=LLT，算法2 列出了具體的計算流程。

2 加速器設計和優化

2.1 整體架構

加速器采用全硬件的定制化電路，基于FPGA平臺實現。根據算法流程，本文提出了如圖2 所示的硬件架構。誤差、雅可比和成本計算單元（error，Jacobian and cost calculation unit，EJCC）可工作在誤差、雅可比計算（error and Jacobian calculation，EJC）模式和成本計算（cost calculation，CC）模式。EJC 模式計算IMU 誤差項和誤差對狀態的導數J。海森矩 陣 和 梯 度 計 算 單 元（Hessian matrix and gradient calculation unit，HGC）計算海森矩陣JTJ和梯度JT?，并將其存儲到片上存儲中。線性方程組求解單元（linear equation solving unit，LES）使用喬可斯基分解算法求解正規方程，解得狀態的增量 δX。EJCC 工作在CC 模式計算更新后的成本F(Xnew)。狀態更新和信任域調整單元判斷是否滿足F(Xnew)小 于F(X)：若滿足，則將Xnew輸入到EJCC 中進行下一次迭代，并縮小 μ；若不滿足，則增大 μ并重新求解正規方程。

圖2 硬件設計架構Fig.2 Hardware design architecture

本文通過分析算法流程和數據的特點進行整體架構、電路實現及存儲的優化。

利用算法中的相同計算實現電路和數據復用。電路復用體現在EJC 和CC 運算使用EJCC 電路的不同模式實現，此外在2 種模式下實現流水線的平衡。數據復用體現在使用誤差、雅可比通用計算模塊計算通用部分，計算結果同時用于誤差和雅可比的計算，實現數據的復用，提高加速器的效率。

利用雅可比矩陣和海森矩陣的稀疏性進行計算和存儲的優化。HGC 電路利用雅可比矩陣的稀疏性實現JTJ的運算簡化，片上存儲利用海森矩陣的稀疏特點實現存儲的壓縮。

線性方程組求解包含矩陣分解和三角線性方程求解等步驟，是電路中延時最長的運算，平均占系統運算總時間的82.4%。本文設計了矩陣分解單元的數量可配置的LES 電路，該配置參數對加速器的性能和資源消耗影響明顯，通過調整該參數可實現加速器性能、資源使用和功耗的折中。

2.2 電路和數據的復用

復用體現在EJCC 模塊，首先介紹誤差和雅可比的計算方法。誤差的計算如式(1)所示，對于N個狀態，需計算N?1 個誤差項，每個誤差項ri,j(xi,xj,yi,j)是15 維的IMU 誤差向量。雅可比矩陣J是N?1 個誤差項對狀態變量X的偏導數。對于誤差項ri,j，與其相關的優化變量是xi、xj（30 維），因此誤差項ri,j對狀態X的偏導數是15×30 的雅可比矩陣，記為Ji,j?；贔orster 等[14]提出的求導方法和具體的Ji,j的解析形式，本文根據誤差項和狀態的物理含義將Ji,j表示成如圖3 所示的分塊矩陣形式。

圖3 誤差函數對狀態量的雅可比矩陣Fig.3 Jacobian matrix of error function versus states

圖3 中，每個矩陣分塊表示對應的誤差分量對狀態分量的導數，如B1表示誤差項中平移誤差對旋轉狀態的導數。PI 由前端提供，E為單位矩陣，Ri為qi的旋轉矩陣形式。B1～B6根據導數的解析式計算，空白部分的值是0。

LM 算法迭代過程中，需要先后計算成本F(X)和誤差、雅可比，如算法1 的第11 行和13 行?？紤]到成本是誤差向量的范數平方和，本文利用EJCC的2 種工作模式實現成本和誤差雅可比的計算。EJCC 的電路架構如圖4 上部所示。該電路分為3 個模塊，模塊之間的數據交互通過Ping-Pong RAM 實現。EJCC 可工作在如圖4 所示的EJC 和CC 這2 種模式。模式1 用于計算誤差和雅可比，將成本計算單元關閉，分為2 個流水階段，階段1 的2 個電路模塊分別完成誤差、雅可比通用項的計算和雅可比矩陣分塊的計算；階段2 完成誤差項的計算，并將雅可比矩陣寫入到片上存儲。模式2 無需計算雅可比，因此關閉雅可比計算和雅可比寫入模塊，開啟成本計算模塊，分成3 個流水階段，階段1 和階段2 計算誤差，階段3 求誤差的平方和得到成本。

圖4 誤差、雅可比和成本計算電路及配置模式Fig.4 Error, Jacobian and cost calculation circuit and its configuration modes

EJCC 實現2 種模式下流水線的平衡，使得2 種模式均可以高效運行。雅可比寫入模塊的延時最長，將其作為一個單獨的流水階段。在EJC 模式中，將誤差、雅可比通用計算模塊和雅可比計算模塊劃分為階段1，使得兩階段的延時近似相等。誤差計算延時和誤差、雅可比通用計算的延時相等，將誤差計算劃分到階段2。這樣在關閉了雅可比相關計算模塊的CC 模式下，階段1 和階段2 延時相同，成本計算模塊通過調節乘累加單元的數目與前2 個階段平衡，CC 模式實現流水線的平衡。

誤差和雅可比的計算中存在相同的中間值，本文據此重構了誤差、雅可比的計算流程。圖5 為EJCC 階段1 電路，在硬件上實現了誤差、雅可比通用計算模塊計算共同的中間值，其中QM 表示四元數的乘法運算。EJCC 階段1 電路分為3 個并行部分，同時計算不同的中間值和圖3 中雅可比矩陣的分塊。

圖5 誤差、雅可比和成本計算階段1 電路Fig.5 Stage one of error, Jacobian and cost calculation circuit

第1 部分的電路處理誤差、雅可比中與Δp和Δv相關部分，計算平移中間值 (pj?pi?vi?t+0.5G?t2)、速度中間值 (vj?vi+G?t)及雅可比的B1和B6分塊；第2 部分電路計算誤差和雅可比中都用到的旋轉矩陣Ri，同時計算Ri與Δt的乘積即雅可比的B2分塊；第3 部分計算與Δq相關的導數和誤差，誤差、雅可比通用計算模塊計算中間值 ?q?(qi?qj)，旋轉雅可比計算單元計算B3～B5分塊。中間值不僅直接用于計算雅可比，還保存下來用于下一階段的誤差計算。該電路實現了誤差、雅可比的數據復用，避免重復計算，提高了計算的效率。

2.3 利用稀疏性優化計算和存儲

HGC 單元計算海森矩陣JTJ，本文利用雅可比矩陣的稀疏結構重新劃分矩陣分塊來減少計算量。定義圖3 矩陣左上角前9 行、前24 列的子矩陣為Λ，通過合并單位矩陣和0 矩陣將Ji,j寫成式(3)的分塊形式，其中E6表示6 維的單位矩陣。

海森矩陣的維度是15N，包含(15N)2個元素，直接存儲需要消耗大量的存儲資源。本文利用算法特點、海森矩陣的性質和矩陣塊的稀疏性，分3 個步驟壓縮海森矩陣的存儲，圖6 以5 個優化變量為例進行說明。首先，海森矩陣中的值表示狀態的約束關系，IMU 誤差僅約束相鄰的狀態，因此不相鄰的狀態如x1和x3在海森矩陣中的矩陣塊是零。其次，海森矩陣是對稱矩陣，僅存儲下三角矩陣可以減少近一半的存儲。最后，利用每一個矩陣分塊的細粒度的稀疏特性，去除矩陣結構中固定位置的0 元素的存儲。圖7 展示了具有20 和80 個優化變量的海森矩陣的壓縮效果，經過3 個步驟的存儲壓縮后，可以分別節約21.1 倍和82.8 倍的存儲資源。

圖6 H 矩陣的存儲優化Fig.6 Storage optimization of H matrix

圖7 在20 個和80 個優化狀態下的存儲優化效果Fig.7 Storage optimization results with 20 and 80 optimization states

2.4 適用多場景要求的可配置設計

LES 的電路架構如圖8 所示。首先喬可斯基分解模塊將系數矩陣分解為下三角矩陣L，然后通過矩陣轉置模塊得到LT。最后利用三角矩陣線性方程求解單元先后求解Lz=g和LTδX=z來求得原方程的解 δX。

圖8 喬可斯基分解算法線性方程組求解電路Fig.8 Cholesky decomposition linear equation solving circuit

喬可斯基分解是求解線性方程組中計算復雜度最高的步驟。該電路采用可配置的設計來實現性能、功耗和資源使用的靈活可變，滿足不同的場景約束。本文設計的喬可斯基分解電路如圖8 上部所示，其級聯了多個喬可斯基分解基本單元（Cholesky process element，CPE）。CPE 包括2 個電路模塊：求值和更新。求值模塊對應算法2 的第2 行和第4 行，計算下三角矩陣L第i列的值，并存儲到求值模塊中的片上存儲器中。更新模塊對應算法2 的第6 行，更新被分解的矩陣S的第i+1～n列，將更新結果存儲到FIFO 中提供給下一級的CPE 讀取。

圖9 以配置4 個CPE 為例，展示了各級CPE 的計算時序。第1 級CPE 的求值模塊計算出L的第1 列L1，更新模塊更新矩陣S的第2～n列，作為第2 級CPE 的輸入。第1 級CPE 更新的結果開始寫入到第1 級和第2 級之間的FIFO 中后第2 級CPE即可開始求值運算。同樣，第2 級到第4 級CPE 依次計算L的2 到4 列并不斷更新S。待第1 級CPE更新完成后，開始第2 輪計算。首先使用第1 級CPE的求值模塊得到L的第5 列L5，第1 級輸出更新的運算結果后第2 級CPE 求得L的第6 列L6。第3、4 級CPE 在上一級更新開始后計算L7 和L8。每一輪求得L的4 列，直到求得完整的L計算結束。

圖9 喬可斯基分解基本單元的計算時序Fig.9 Computing schedule of CPEs

由以上分析可知，分解完成一個n維矩陣需要運行的輪數為n/#CPE，其中，#CPE表示CPE 的級數。增加CPE 的級數可以增加輪內的計算并行度，減少運行輪數，縮短計算的時間。隨著 #CPE的增加，資源消耗線性增加，由于輪數與 #CPE是反比關系，運行時間不斷減小，但減小速度變緩。本文的設計將CPE 的數目參數化，調節該參數實現性能、資源使用和功耗的折中。具體來講，系統中的狀態數目確定后，如果速度無法滿足當前場景的需求，則增大參數值來提高運行速度；如果當前場景對功耗和資源占用限制嚴格，則減小配置參數的值以減小功耗。配置參數的修改需要重新綜合新的加速器硬件電路并在FPGA 上進行配置。

3 實驗結果和分析

本文使用Xilinx ZC706 平臺[15]對硬件設計進行評估，時鐘頻率為143 MHz。本文設計與SLAM軟件VINS-Fusion[16]的后端優化進行對比，該軟件使用谷歌公司的開源非線性優化庫Ceres[17]進行狀態優化。IMU 求導運算和方程求解的數值均需要大的動態范圍和高精度[18]，為了保證系統的穩定性和定位精度，本文評估的軟硬件均使用32 bit 浮點數表示數據。在2 個處理器平臺上對軟件進行評估：搭 載Intel i5-8250U 處 理 器，主 頻1.6 GHz 的x86 平臺；搭載四核Arm Cortex-A57 處理器，主頻1.9 GHz 的Nvidia Jetson TX1 平 臺[19]。為 比 較 軟 硬件性能，使用EuRoC 數據集[20]進行測試，該數據集采集自微型飛行器（micro aerial vehicle，MAV）上的視覺慣性傳感器單元，其中包含IMU（ADIS16448，200 Hz）的加速度和角速率測量數據。首先使用VINS-Fusion 前端計算IMU 的積分值，后端優化選取20、50、80 個IMU 狀態的測試數據，即系統的狀態數N為20、50 和80。

調整LES 模塊中CPE 的個數對整體的性能、資源消耗和功耗有明顯的影響。在狀態數目N為50 時，矩陣分解單元的數量設置為最小5 和最大120 的情況下，IMU 狀態優化加速器的運行時間分別為171 ms 和8.97 ms。前者的運行時間為后者的19.1 倍；FPGA 資源使用方面，最大參數配置下的加速器中數字信號處理單元和存儲資源分別是最小參數配置的6.4 倍和11.4 倍。本文在硬件上實現了3 個不同配置的電路C1、C2、C3，配置的CPE 的個數分別為20、40、80。C1、C2、C3 的硬件并行度依次提高，性能提升的同時資源消耗和功耗也增加。

硬件的資源消耗數據使用Vivado 工具布局布線后報告得出。C1、C2、C3 的資源消耗如表1 所示，3 種配置電路占用了FPGA 芯片中13.3%～27.3%的查找表（look up table，LUT）、6.7%～12.4%的觸發器（flip-flop，FF）、15.9%～32.4%的塊隨機存取存儲器RAM（block-RAM，BRAM）和30.9%～70.9%的數字信號處理單元（digital signal processor，DSP）。其中，BRAM 的數量以36 Kbit 塊為單位，0.5 表示18 Kbit 塊。評估使用的硬件平臺中DSP 資源共有900 個，CPE 單元中的更新模塊包含浮點乘和浮點加運算，增加一個CPE 單元DSP 的使用增加6 個，占總數的0.67%，CPE 數目的增多使DSP 的資源占用變化最大。每一個CPE 中帶有存儲矩陣分解結果的片上存儲器，BRAM 的資源占用隨著CPE 的數目的增多而變高。此外，BRAM 還和硬件能處理的IMU 狀態數有關，本文設計能處理的最大的IMU狀態數為80。

表1 硬件設計的資源消耗Table 1 Resource utilization of hardware design

對3 種配置的電路在不同測試數據上的性能進行了評估，如表2 所示。測試數據中，隨著狀態數目N變大，3 種配置電路的執行時間均變長。對IMU 狀態數目小，如N=20 的測試數據，FPGA 相比軟件取得較大的加速效果，處理時間小于1 ms，相比于x86 可以達到67.2 倍的加速效果，相比于TX1平臺可以達到190.3 倍的加速效果。隨著測試數據規模變大，FPGA 的加速效果減小，這是由于軟件中分配的處理資源如內存等會同時增加，而FPGA 的電路使用的資源沒有變化。但通過增加CPE 的數目，速度可以獲得較大的提升，如對于N=80 的測試數據，C3 的時間相比于C1 的時間減小62.5%。對于不同測試數據上的平均加速效果，低功耗的C1電路相比于x86 平臺和TX1 平臺可以達到平均17.4倍和53.7 倍的加速效果，高性能的C3 電路的加速分別為26.7 倍和87 倍。

表2 軟硬件實現的性能比較Table 2 Performance comparison between hardware and software implementations

此外，本文還對3 個平臺上的功耗和能耗進行了評估。FPGA 功耗使用Vivado 的功耗評估器得出，C1、C2、C3 的功耗分別為2.293 W、2.956 W 和3.759 W。Intel CPU 的功耗評估采用其熱設計功耗（thermal design power，TDP）15 W，Jetson TX1 使 用板載功耗監視傳感器測得功耗為5.02 W。

表3 展示了在3 個測試數據上FPGA 與軟件的能耗，在IMU 狀態數少，如N=20 的測試中，不同配置的電路能耗相當，C3 由于功耗高能耗最多。狀態數目增多，在N=50，80 的測試中，由于C1、C2 和C3 的計算時間差別明顯，C3 的能耗最小。本文實現的FPGA 設計相比x86 平臺和TX1 平臺分別可以節省90%和95%以上的能耗，在高性能的同時也節約了能耗，可以更好地滿足功耗受限、實時性要求高的應用要求。

表3 能耗對比Table 3 Comparison of energy consumption mJ

4 結 論

1）提出了基于FPGA 實現的IMU 狀態優化加速器設計，利用算法流程和數據的特點實現電路復用、計算簡化和存儲壓縮。

2）解方程模塊采用可配置設計，通過改變該模塊的配置可使整體加速器在大范圍的設計空間中變化，以適用不同的場景約束。

3）實驗結果證明，本文設計在性能和功耗方面均大幅優于軟件實現。本文提出的加速器設計可以高效地在功耗受限的嵌入式設備上完成IMU 觀測信息的優化求解。