劉強(qiáng)，劉威壯，俞波，劉少山

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

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

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

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

本文在FPGA 上設(shè)計(jì)了用于IMU 狀態(tài)優(yōu)化的SLAM 后端加速器。加速器采用LM 算法，為算法中的雅可比矩陣計(jì)算、海森矩陣計(jì)算和求解線性方程組等步驟定制硬件計(jì)算架構(gòu)，利用算法流程和數(shù)據(jù)的特點(diǎn)實(shí)現(xiàn)復(fù)用。同時(shí)，充分利用IMU 狀態(tài)優(yōu)化問題中特有的稀疏特性，簡化矩陣計(jì)算和存儲(chǔ)。對于計(jì)算量最大的求解線性方程組的硬件，采用電路規(guī)?？膳渲玫脑O(shè)計(jì)，實(shí)現(xiàn)電路性能、資源消耗和功耗的折中。

1 算法原理

1.1 優(yōu)化問題描述

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

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

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

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

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

1.2 優(yōu)化問題求解

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

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

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

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

2 加速器設(shè)計(jì)和優(yōu)化

2.1 整體架構(gòu)

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

圖2 硬件設(shè)計(jì)架構(gòu)Fig.2 Hardware design architecture

本文通過分析算法流程和數(shù)據(jù)的特點(diǎn)進(jìn)行整體架構(gòu)、電路實(shí)現(xiàn)及存儲(chǔ)的優(yōu)化。

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

利用雅可比矩陣和海森矩陣的稀疏性進(jìn)行計(jì)算和存儲(chǔ)的優(yōu)化。HGC 電路利用雅可比矩陣的稀疏性實(shí)現(xiàn)JTJ的運(yùn)算簡化，片上存儲(chǔ)利用海森矩陣的稀疏特點(diǎn)實(shí)現(xiàn)存儲(chǔ)的壓縮。

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

2.2 電路和數(shù)據(jù)的復(fù)用

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

圖3 誤差函數(shù)對狀態(tài)量的雅可比矩陣Fig.3 Jacobian matrix of error function versus states

圖3 中，每個(gè)矩陣分塊表示對應(yīng)的誤差分量對狀態(tài)分量的導(dǎo)數(shù)，如B1表示誤差項(xiàng)中平移誤差對旋轉(zhuǎn)狀態(tài)的導(dǎo)數(shù)。PI 由前端提供，E為單位矩陣，Ri為qi的旋轉(zhuǎn)矩陣形式。B1～B6根據(jù)導(dǎo)數(shù)的解析式計(jì)算，空白部分的值是0。

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

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

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

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

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

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

2.3 利用稀疏性優(yōu)化計(jì)算和存儲(chǔ)

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

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

圖6 H 矩陣的存儲(chǔ)優(yōu)化Fig.6 Storage optimization of H matrix

圖7 在20 個(gè)和80 個(gè)優(yōu)化狀態(tài)下的存儲(chǔ)優(yōu)化效果Fig.7 Storage optimization results with 20 and 80 optimization states

2.4 適用多場景要求的可配置設(shè)計(jì)

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

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

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

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

圖9 喬可斯基分解基本單元的計(jì)算時(shí)序Fig.9 Computing schedule of CPEs

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

3 實(shí)驗(yàn)結(jié)果和分析

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

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

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

表1 硬件設(shè)計(jì)的資源消耗Table 1 Resource utilization of hardware design

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

表2 軟硬件實(shí)現(xiàn)的性能比較Table 2 Performance comparison between hardware and software implementations

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

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

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

4 結(jié) 論

1）提出了基于FPGA 實(shí)現(xiàn)的IMU 狀態(tài)優(yōu)化加速器設(shè)計(jì)，利用算法流程和數(shù)據(jù)的特點(diǎn)實(shí)現(xiàn)電路復(fù)用、計(jì)算簡化和存儲(chǔ)壓縮。

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

3）實(shí)驗(yàn)結(jié)果證明，本文設(shè)計(jì)在性能和功耗方面均大幅優(yōu)于軟件實(shí)現(xiàn)。本文提出的加速器設(shè)計(jì)可以高效地在功耗受限的嵌入式設(shè)備上完成IMU 觀測信息的優(yōu)化求解。