成 祥,陳遲曉,翟 鵬,張立華

1(復(fù)旦大學(xué) 智能機(jī)器人研究院,上海 200433)

2(上海智能機(jī)器人工程技術(shù)研究中心,上海 200433)

3(智能機(jī)器人教育部工程研究中心,上海 200433)

4(季華實(shí)驗(yàn)室,廣東 佛山 528000)

5(吉林省人工智能與無(wú)人系統(tǒng)工程研究中心,長(zhǎng)春 130000)

E-mail:lihuazhang@fudan.edu.cn

1 引 言

自1980年以來(lái),同步定位和建圖(Simultaneous Localization and Mapping,SLAM)一直是計(jì)算機(jī)視覺(jué)和機(jī)器人技術(shù)領(lǐng)域的活躍研究領(lǐng)域.SLAM為機(jī)器人提供了一種用戶(hù)創(chuàng)建地圖的替代方案,允許機(jī)器人在沒(méi)有預(yù)先獲得場(chǎng)景的本地化基礎(chǔ)結(jié)構(gòu)的情況下,進(jìn)行機(jī)器人的定位和建圖.經(jīng)典的視覺(jué)SLAM系統(tǒng)的體系結(jié)構(gòu)包括前端視覺(jué)里程計(jì)、后端優(yōu)化、回環(huán)檢測(cè)以及建圖4個(gè)組件,其中后端優(yōu)化對(duì)SLAM系統(tǒng)的定位精度和建圖質(zhì)量有著重要的作用[1-3].

后端優(yōu)化對(duì)于整個(gè)視覺(jué)SLAM系統(tǒng)尤為重要,優(yōu)化求解的速度與求解能力直接影響到視覺(jué)SLAM的實(shí)時(shí)性及準(zhǔn)確性,然而實(shí)際應(yīng)用中,視覺(jué)SLAM后端優(yōu)化環(huán)節(jié)需要大量的計(jì)算資源,耗費(fèi)大量的計(jì)算時(shí)間.近些年來(lái),一方面,視覺(jué)SLAM算法執(zhí)行后端優(yōu)化步驟的頻率受到了硬件設(shè)備算力的限制,開(kāi)發(fā)者不得不通過(guò)降低后端優(yōu)化的頻率,從而在實(shí)時(shí)性與性能之間尋找平衡點(diǎn),然而這在一定程度上影響了SLAM算法的工作性能;另一方面,視覺(jué)SLAM算法對(duì)計(jì)算資源的高要求使得部署視覺(jué)SLAM算法的機(jī)器人對(duì)硬件設(shè)備要求很高,造成機(jī)器人設(shè)計(jì)和應(yīng)用的成本大幅提高.現(xiàn)有技術(shù)實(shí)現(xiàn)SLAM算法通常使用通用處理器(CPU)或圖形處理器(GPU)進(jìn)行計(jì)算,通用性差、效率較低且成本較高.

針對(duì)上述問(wèn)題,本文提出了針對(duì)視覺(jué)SLAM后端優(yōu)化的一種通用加速架構(gòu),實(shí)現(xiàn)了基于光束平差法后端優(yōu)化部分的硬件運(yùn)算加速,能夠靈活適用于各種視覺(jué)SLAM算法的后端優(yōu)化部分的運(yùn)算,進(jìn)而可以減少運(yùn)算時(shí)間,從而有望降低機(jī)器人設(shè)計(jì)和應(yīng)用的門(mén)檻,提升視覺(jué)SLAM系統(tǒng)性能,拓展機(jī)器人的應(yīng)用空間.

2 相關(guān)工作

當(dāng)前,SLAM算法的算力需求與處理器運(yùn)算能力之間的矛盾日益顯著.在過(guò)去的幾十年間,SLAM算法因其計(jì)算復(fù)雜度和實(shí)時(shí)性要求只能在高性能計(jì)算機(jī)上執(zhí)行.隨著摩爾定律的發(fā)展到達(dá)瓶頸期,通用嵌入式處理器的運(yùn)算能力已達(dá)到飽和,約為5GOP/s[4](1)https://zenodo.org/record/3947824#.YA6Gq6j7SUk,然而快速發(fā)展的SLAM新算法對(duì)硬件平臺(tái)的算力需求已從5GOP/s上升到50GOP/s,SLAM算法與深度學(xué)習(xí)/語(yǔ)義理解結(jié)合,運(yùn)算能力的要求將達(dá)到500GOP/s.其中SLAM后端運(yùn)算占據(jù)總運(yùn)算需求的50%[5-9].

研究者對(duì)SLAM加速問(wèn)題進(jìn)行了長(zhǎng)久的探索.Jeong等人[10]利用SLAM問(wèn)題的稀疏特性提出了簡(jiǎn)化相機(jī)模型系統(tǒng),并基于塊稀疏矩陣加速的特性加速了SLAM的處理速度.Wu[11]等人提出了針對(duì)CPU和GPU的多核加速方案,通過(guò)預(yù)處理共軛梯度下降法優(yōu)化矩陣向量乘積減少了內(nèi)存大小,并且該方法更有利于并行化運(yùn)算.Eriksson等人[12]以及Zhang等人[13]提出了針對(duì)大規(guī)模光束平差法的分布式計(jì)算方法,并通過(guò)分割相機(jī)共視地圖點(diǎn)的方法減少了分布式計(jì)算的通信開(kāi)銷(xiāo).這些探索都是在通用處理器或圖形處理器上開(kāi)展,盡管有一定發(fā)展,但沒(méi)有很好的利用到光束平差法在優(yōu)化內(nèi)存和計(jì)算資源以及加快運(yùn)算速度方面的優(yōu)勢(shì).

此外,一些研究者在硬件加速器方面嘗試加速SLAM算法.eSLAM[14]提出了一種在FPGA平臺(tái)上加速特征提取和匹配階段的實(shí)時(shí)ORB SLAM的節(jié)能架構(gòu),從而實(shí)現(xiàn)了在低功耗平臺(tái)上實(shí)現(xiàn)實(shí)時(shí)SLAM算法.Xu等人[15]提出了基于CNN的特征點(diǎn)提取的SLAM加速器來(lái)加速前端特征提取速度.Schulz等人[16]基于ARM的嵌入式FPGA SoC實(shí)現(xiàn)了用于視覺(jué)SLAM的立體視覺(jué)預(yù)處理模塊.上述文獻(xiàn)[14-16]中的SLAM加速器僅僅對(duì)前端進(jìn)行加速,而沒(méi)有考慮運(yùn)算需求更大的后端運(yùn)算.一些研究者對(duì)特定的SLAM算法在嵌入式計(jì)算平臺(tái)進(jìn)行實(shí)現(xiàn),如Sugiura[17]針對(duì)2D LiDAR的GMapping算法提出了一種并行加速掃描與匹配的加速器,Sileshi等人[18]以及Abouzahir等人[19]實(shí)現(xiàn)了基于粒子過(guò)濾器的Fast SLAM的FPGA加速器Tertei等[20]人以及Hanif等人[21]實(shí)現(xiàn)了針對(duì)3D Visual SLAM的EKF-SLAM加速器塊,Gautier等人[19]針對(duì)視覺(jué)、激光融合SLAM算法框架InfiniTAM提出了加速深度融合和射線投射模塊的FPGA架構(gòu).密歇根大學(xué)提出了一個(gè)加速半全局匹配過(guò)程的并行處理器[22],能夠?qū)崿F(xiàn)密集的實(shí)時(shí)3D深度和3D運(yùn)動(dòng)感知的擺姿勢(shì)設(shè)計(jì)可在全高清(1920×1080,FHD)分辨率下實(shí)現(xiàn)基于相鄰像素的半全局匹配,只加速SLAM運(yùn)算量較小的姿態(tài)估計(jì)部分.特文特大學(xué)使用FPGA設(shè)計(jì)了GraphSLAM的并行化系數(shù)求解架構(gòu)[23],可以執(zhí)行共軛梯度算法和稀疏矩陣的點(diǎn)積、加法、縮放操作,但是無(wú)法實(shí)現(xiàn)對(duì)運(yùn)算需求更大、操作更頻繁的矩陣乘法的運(yùn)算,同時(shí)框架不能支持SLAM后端優(yōu)化中更普遍使用的光束平差法.

總之,現(xiàn)有技術(shù)缺乏有效的、靈活且通用的SLAM算法后端優(yōu)化部分加速方法和裝置,尚未存在一款用于后端優(yōu)化流程的可以靈活配置的專(zhuān)用SLAM硬件加速結(jié)構(gòu).

3 算法基本原理

3.1 相機(jī)模型

在計(jì)算機(jī)視覺(jué)領(lǐng)域,相機(jī)模型通常利用幾何知識(shí)和笛卡爾坐標(biāo)系建模,通過(guò)小孔成像原理,將理想成像過(guò)程轉(zhuǎn)化成通過(guò)3個(gè)方向上的平移和旋轉(zhuǎn)實(shí)現(xiàn),最終將現(xiàn)實(shí)世界的三維空間點(diǎn)投影到二維圖像像素點(diǎn).當(dāng)相機(jī)模型建模用齊次坐標(biāo)形式進(jìn)行描述,可以表述為公式(1):

(1)

其中,K為相機(jī)內(nèi)參數(shù)矩陣,T∈4×4為相機(jī)位姿變換矩陣,T可以分解成旋轉(zhuǎn)矩陣R∈3×3和位移矩陣t=[t1,t2,t3]T兩部分構(gòu)成,Pw=[X,Y,Z]T為當(dāng)前世界坐標(biāo)中的點(diǎn),Puv=[u,v,1]T為圖像坐標(biāo)系中的點(diǎn),Z為世界坐標(biāo)系中的z軸坐標(biāo).

3.2 光束平差法

視覺(jué)SLAM中,在將估計(jì)產(chǎn)生的軌跡和地圖帶入到SLAM的運(yùn)動(dòng)和觀測(cè)模型中時(shí),傳感器誤差、里程計(jì)積累誤差以及特征點(diǎn)之間的誤差等誤差會(huì)導(dǎo)致模型不完美,隨著時(shí)間的積累,甚至可能導(dǎo)致SLAM模型中的軌跡發(fā)生錯(cuò)誤、地圖嚴(yán)重失準(zhǔn)等問(wèn)題.解決這一問(wèn)題的一種有效手段是,通過(guò)局部?jī)?yōu)化或全局優(yōu)化來(lái)微調(diào)模型參數(shù),不斷縮小視覺(jué)SLAM系統(tǒng)誤差達(dá)到一個(gè)在允許范圍內(nèi)的極小值,從而使視覺(jué)SLAM模型在長(zhǎng)時(shí)間的運(yùn)行過(guò)程中依舊保持魯棒性和準(zhǔn)確性.局部?jī)?yōu)化或全局優(yōu)化問(wèn)題通常被表述為非線性最小二乘問(wèn)題,誤差定義為觀測(cè)到的特征位置與相應(yīng)3D點(diǎn)在相機(jī)圖像平面上的投影位置之間差值的平方,優(yōu)化方程如公式(2)所示:

(2)

其中,x為待優(yōu)化參數(shù)的向量,由相機(jī)位姿向量xc∈SE(3)和特征點(diǎn)參數(shù)向量xp∈3組成,即是3D重建的重投影誤差的向量,x*是優(yōu)化求解后的最優(yōu)參數(shù)向量.

光束平差法是求解視覺(jué)SLAM后端非線性?xún)?yōu)化問(wèn)題的一個(gè)經(jīng)典方法,它能夠精確的優(yōu)化每一個(gè)相機(jī)位姿以及每一個(gè)地圖特征點(diǎn)的位置.光束平差法的算法基本思想是,假設(shè)在一次觀測(cè)中,場(chǎng)景中的所有觀測(cè)點(diǎn)都會(huì)反射出光束,算法通過(guò)優(yōu)化實(shí)際觀測(cè)點(diǎn)和估計(jì)點(diǎn)之間的重投影誤差調(diào)整相機(jī)位姿以及地圖特征點(diǎn)位置,使得所有反射產(chǎn)生的光束最終都可以交匯于一點(diǎn),即所有光束最終都可以由相機(jī)發(fā)出,進(jìn)而構(gòu)建視覺(jué)SLAM后端優(yōu)化模型.

3.3 Levenberg-Marquardt方法

迭代求解算法Levenberg-Marquardt(LM)[1-3]對(duì)于解決非線性最小二乘問(wèn)題有著良好的性能,它結(jié)合了高斯牛頓迭代法和最速下降法,并采用二階泰勒展開(kāi)近似,同時(shí)考慮使用置信區(qū)域來(lái)判斷泰勒近似的程度,其求解公式如公式(3)所示:

(3)

其中,J(x)為f(x)的雅可比矩陣,D(x)是非負(fù)對(duì)角矩陣,由對(duì)角線矩陣J(x)TJ(x)的平方根計(jì)算所得,λ為控制正則化強(qiáng)度的拉格朗日乘子.如果‖f(x+δ*)‖<‖f(x)‖,則更新x←x+δ*,通過(guò)λ控制更新量的大小.當(dāng)λ較大時(shí),公式近似為最速下降法,當(dāng)λ較小時(shí),公式近似為高斯牛頓迭代法.參數(shù)λ依據(jù)雅可比矩陣J(x)逼近f(x)的程度來(lái)更新.相比高斯牛頓法,LM算法的求解方式能夠提供更穩(wěn)定更準(zhǔn)確的增量值δ*.

求解公式(3)等同于求解方程,如公式(4)所示:

(JTJ+λDTD)δ=-JTf(x)=

(4)

光束平差法的優(yōu)化方程,可以重寫(xiě)為矩陣形式.記Hλ=JTJ+λDTD,Hλ為N×N的矩陣,N=6n+3m.令U=JcT,V=JpTJp,Uλ=U+λDcTDc,Vλ=V+λDpTDp,W=JcTJp,則公式(4)可以重寫(xiě)為公式(5):

(5)

其中,Uλ是對(duì)角塊矩陣,每個(gè)塊為6×6的矩陣,只與相機(jī)姿態(tài)有關(guān),對(duì)角線共有n個(gè)小矩陣塊;Vλ同樣是對(duì)角塊矩陣,每個(gè)塊為3×3的矩陣,只與地圖特征點(diǎn)有關(guān),對(duì)角線共有m個(gè)小矩陣塊;矩陣W與WT矩陣互為轉(zhuǎn)置,其稀疏情況由具體的相機(jī)位姿所觀測(cè)到的數(shù)據(jù)決定.

3.4 舒爾消解

在SLAM后端優(yōu)化步驟中,相機(jī)位姿參數(shù)的個(gè)數(shù)n一般維護(hù)在不超過(guò)100,而地圖點(diǎn)的數(shù)量m能夠達(dá)到10000以上,即使使用關(guān)鍵幀以及稀疏性的特征,每個(gè)關(guān)鍵幀通常能夠觀測(cè)到超過(guò)2000個(gè)關(guān)鍵點(diǎn),即n?m.這就導(dǎo)致公式(5)中的矩陣規(guī)模很大,在求解運(yùn)算過(guò)程中耗費(fèi)大量算力.

公式(5)具有稀疏化的特性,這是由雅可比矩陣的稀疏性所決定的.光束平差法的關(guān)鍵特征是,待優(yōu)化函數(shù)誤差項(xiàng)僅與觀測(cè)相機(jī)位姿所觀測(cè)得到的地圖特征點(diǎn)位置有關(guān)——當(dāng)僅考慮涉及到的第i個(gè)相機(jī)以及其可以觀測(cè)到的第j個(gè)地圖點(diǎn)特征點(diǎn)時(shí),雅可比矩陣在只涉及第i個(gè)相機(jī)參數(shù)和第j個(gè)路標(biāo)點(diǎn)的項(xiàng)中不為零,其余項(xiàng)全部為零.公式(6)描述了上述雅可比矩陣特性:

(6)

由于雅可比矩陣的稀疏化的特性,公式(5)第1項(xiàng)存在大量的零矩陣塊,其中矩陣塊Uλ和矩陣塊Vλ除對(duì)角線分別存在關(guān)于相機(jī)位姿的6×6的矩陣塊和關(guān)于地圖點(diǎn)特征點(diǎn)的3×3的矩陣塊外,其余均為零矩陣塊;矩陣塊W以及其轉(zhuǎn)置WT也同樣存在大量零矩陣塊,只有在涉及第i個(gè)相機(jī)參數(shù)和第j個(gè)路標(biāo)點(diǎn)的項(xiàng)存在非零數(shù)據(jù),可以將非對(duì)角線上的非零矩陣塊形象地當(dāng)作是變量之間存在的約束條件.

(7)

4 硬件架構(gòu)

4.1 SLAM后端專(zhuān)用并行化計(jì)算架構(gòu)

本文提出一種面向光束平差法的視覺(jué)SLAM并行計(jì)算硬件架構(gòu),通過(guò)加速后端優(yōu)化部分,能有效加速采用了光束平差法的各種視覺(jué)SLAM算法,如圖1所示.FPGA包含處理系統(tǒng)(Processing System, PS)和可編程系統(tǒng)(Programmable Logic, PL)兩部分,PL部分包括用于加速SLAM中的矩陣乘加運(yùn)算單元、舒爾補(bǔ)構(gòu)造單元以及預(yù)處理共軛梯度下降法求解器單元,PL部分的3類(lèi)單元分別用于矩陣乘加操作、舒爾消解過(guò)程與正定矩陣方程求解過(guò)程,PS部分執(zhí)行基于光束平差法的視覺(jué)SLAM后端優(yōu)化部分其他步驟.

圖1 光束平差法加速器硬件架構(gòu)Fig.1 Architecture of bundle adjustment accelerator

PS部分與PS部分之間通過(guò)總線通信,由于SLAM后端數(shù)據(jù)流復(fù)雜,因此不同模塊分時(shí)復(fù)用,并采用流水線設(shè)計(jì)方式,由通用處理器進(jìn)行控制和調(diào)度,其中包括以下4個(gè)步驟:

步驟1.預(yù)計(jì)算Hλ的相關(guān)矩陣,此步驟將由矩陣乘加運(yùn)算單元實(shí)際計(jì)算和加速;

步驟2.通過(guò)專(zhuān)用舒爾補(bǔ)構(gòu)造加速單元并行地構(gòu)造舒爾補(bǔ)矩陣S、bschur;

步驟3.利用預(yù)處理共軛梯度方法的專(zhuān)用矩陣迭代計(jì)算加速單元迭代求解矩陣方程,獲得相機(jī)位姿參數(shù)的改變量δc;

步驟4.計(jì)算地圖點(diǎn)空間改變量δp,此步驟也將由矩陣乘加運(yùn)算單元參與加速.

自此,SLAM系統(tǒng)完成了一個(gè)后端優(yōu)化步驟,更新了相機(jī)位姿參數(shù)以及地圖點(diǎn)信息.圖2形象地說(shuō)明了SLAM后端優(yōu)化的4個(gè)步驟中具體硬件與操作的映射關(guān)系.

圖2 硬件與操作的映射關(guān)系Fig.2 Hardware mapping for bundle adjustment ccelerator

4.2 數(shù)據(jù)存儲(chǔ)方式

在SLAM后端優(yōu)化求解過(guò)程中,直接存儲(chǔ)原始數(shù)據(jù)、各類(lèi)矩陣、中間結(jié)果等數(shù)據(jù)將消耗巨大的內(nèi)存空間,且影響數(shù)據(jù)存取及運(yùn)算時(shí)間,所以影響性能的一個(gè)關(guān)鍵設(shè)計(jì)決策是如何將各種矩陣存儲(chǔ)在RAM中.

加速光束平差加速器硬件架構(gòu)的PL部分輸入為投影誤差e=[ec;ep]、相機(jī)參數(shù)雅可比矩陣Jc以及地圖點(diǎn)參數(shù)雅可比矩陣Jp等矩陣,經(jīng)過(guò)運(yùn)算輸出相機(jī)位姿改變量矩陣δc以及地圖特征點(diǎn)改變量矩陣δp.考慮到存在的稀疏的、塊結(jié)構(gòu)的矩陣特性,可以利用塊結(jié)構(gòu)壓縮行稀疏(Block Compressed Sparse Row,BCSR)的方式存儲(chǔ)稀疏的矩陣,該方法能減少存儲(chǔ)空間,有利于并行化運(yùn)算.

典型的BCSR格式使用3個(gè)屬性:按行順序線性存儲(chǔ)的非零塊的值vals,所對(duì)應(yīng)位置原始非零塊的列索引cols,以及每行的第1個(gè)非零塊的在vals陣列中的索引row_blk.在光束平差法中,使用BCSR格式存儲(chǔ)時(shí),vals按行順序線性存儲(chǔ)非零元素塊,cols表示相機(jī)參數(shù)雅可比矩陣Jc以及地圖點(diǎn)參數(shù)雅可比矩陣Jp的非零塊所在列索引.本文與BCSR不同的一點(diǎn)是,使用共視集合數(shù)組(co-visibility set,COi)代替row_blk,共視集合的物理意義為所能觀測(cè)到第i個(gè)點(diǎn)的圖像集合,例如在圖3中,CO4={2,3}即為從圖像2和圖像3中可以觀測(cè)到地圖點(diǎn)4,集合元素的數(shù)目可以作為row_blk記錄每行非零元素起始位置.該改進(jìn)方式包含光束平差法問(wèn)題的物理含義的指針數(shù)組COi,可以直接用于后續(xù)計(jì)算中,而無(wú)需進(jìn)一步轉(zhuǎn)換,有利于在硬件中進(jìn)行并行化計(jì)算.

圖3 改進(jìn)的BCSR數(shù)據(jù)存儲(chǔ)格式Fig.3 Improved BCSR data storage format

算法產(chǎn)生的中間結(jié)果舒爾補(bǔ)矩陣S為稠密的大規(guī)模對(duì)稱(chēng)正定矩陣,本文考慮將該矩陣的分為對(duì)角線上的對(duì)角塊、對(duì)角線以上以及對(duì)角線以下的對(duì)稱(chēng)矩陣部分,如圖3右上角圖片所示,本文在設(shè)計(jì)硬件存儲(chǔ)中僅需存儲(chǔ)舒爾補(bǔ)矩陣S對(duì)角線上以及對(duì)角線以上的部分,這將節(jié)省近一半的存儲(chǔ)空間.

4.3 舒爾補(bǔ)構(gòu)造單元

為了解決求解方程規(guī)模過(guò)大的問(wèn)題,本文嘗試?yán)霉馐讲罘ㄖ蠬λ的稀疏塊結(jié)構(gòu)的特性設(shè)計(jì)專(zhuān)用舒爾補(bǔ)構(gòu)造單元,如圖4所示.由于存在舒爾消解的方式,可以將規(guī)模為N×N(N=3m+6n)的方陣簡(jiǎn)化到規(guī)模為6n的方陣,且通常n?m,這將極大簡(jiǎn)化和加速矩陣方程求解過(guò)程.

圖4 舒爾補(bǔ)構(gòu)造單元硬件架構(gòu)Fig.4 Hardware architecture of schur elimination

舒爾補(bǔ)構(gòu)造單元硬件架構(gòu)依照塊結(jié)構(gòu)的舒爾消解方法進(jìn)行舒爾補(bǔ)矩陣構(gòu)造,該舒爾補(bǔ)構(gòu)造單元的輸入包括投影誤差e、相機(jī)參數(shù)雅可比矩陣Jc以及地圖點(diǎn)參數(shù)雅可比矩陣Jp等矩陣,依據(jù)計(jì)算的數(shù)據(jù)依賴(lài)性可以將計(jì)算過(guò)程分為5個(gè)部分,共計(jì)12個(gè)計(jì)算階段,同時(shí)幾種不同的部分根據(jù)計(jì)算量的大小和數(shù)據(jù)的依賴(lài)性,拆分為5個(gè)計(jì)算階段以平衡計(jì)算延遲,增加計(jì)算速度.舒爾補(bǔ)構(gòu)造單元硬件架構(gòu)如圖4所示.圖中虛線框部分為專(zhuān)用的矩陣乘加單元.第1個(gè)部分計(jì)算地圖點(diǎn)的雅可比矩陣Jp的轉(zhuǎn)置與其本身的矩陣乘加V=∑JpTJp,以及計(jì)算其矩陣的逆V-1;第2個(gè)部分計(jì)算地圖點(diǎn)的雅可比矩陣Jp和投影誤差e的矩陣乘p,之后依次與矩陣V-1、Jp和Jc進(jìn)行矩陣乘加運(yùn)算,得到JcJpV-1p;第3個(gè)部分計(jì)算關(guān)于相機(jī)參數(shù)的雅可比矩陣Jc和投影誤差e的矩陣乘c,與第1部分結(jié)果相減得到bschur=c-∑JcTJpV-1p;第4個(gè)部分計(jì)算相機(jī)參數(shù)的雅可比矩陣Jp的轉(zhuǎn)置與地圖點(diǎn)的雅可比矩陣的轉(zhuǎn)置JpT與的矩陣乘加運(yùn)算WT=∑JpTJc,計(jì)算其轉(zhuǎn)置后,之后依次與矩陣V-1、Jp和Jc進(jìn)行矩陣乘加運(yùn)算,得到JcTJpV-1WT;第5個(gè)部分計(jì)算相機(jī)參數(shù)的雅可比矩陣的轉(zhuǎn)置JcT與其本身的矩陣乘加U=∑JcTJc,之后計(jì)算與第4部分計(jì)算結(jié)果的矩陣減法,得到矩陣Hλ的舒爾補(bǔ)矩陣S=U-∑JcTJpV-1WT.自此,該單元完成了舒爾補(bǔ)消解后的求解方程的構(gòu)造.

舒爾補(bǔ)構(gòu)造單元能夠有效地并行加速的原因有兩個(gè):1)由于雅可比矩陣及海森矩陣具有稀疏的特性,構(gòu)造過(guò)程中矩陣可以按照塊結(jié)構(gòu)劃分,將塊結(jié)構(gòu)作為運(yùn)算的基本單元,矩陣稀疏特性允許只需在涉及到對(duì)應(yīng)部分的塊結(jié)構(gòu)上進(jìn)行矩陣操作,因此可以并行地調(diào)用多個(gè)采用并行加速設(shè)計(jì)的矩陣乘加運(yùn)算單元實(shí)現(xiàn)按塊構(gòu)造;2)該單元將舒爾構(gòu)造流程中無(wú)數(shù)據(jù)依賴(lài)的可并行的過(guò)程進(jìn)行并行設(shè)計(jì),例如圖4中不同計(jì)算階段下的操作可以并行進(jìn)行,這也是舒爾補(bǔ)構(gòu)造單元能夠進(jìn)行并行加速的一個(gè)原因.

4.4 預(yù)處理共軛梯度下降法求解單元

預(yù)處理共軛梯度下降法是一種可以通過(guò)迭代的方式逼近矩陣方程解的求解方法,這種迭代求解允許在計(jì)算過(guò)程中不直接存儲(chǔ)矩陣A,而是使用Ap代替,能夠良好的處理大規(guī)模矩陣方程,收斂性取決于所有特征值的分布情況[24].預(yù)處理共軛梯度下降求解算法如算法1所示.

算法 1.預(yù)處理共軛梯度下降法求解矩陣方程

輸入：Ax=b,A∈n×n,A=AT?0

輸出：x

1.x:=0,r:=b-Ax0,p:=r,z:=Mr,ρq:=rTr

2.fork=1 toNmaxdo

4.w:Ap

6.x:=x+αp

7.r:=r-αw

8.z:=Mr

9.ρk+1:=zTr

11.end for

使用預(yù)處理矩陣技術(shù),在舒爾補(bǔ)上進(jìn)行共軛梯度下降算法,利用舒爾補(bǔ)構(gòu)造實(shí)現(xiàn)對(duì)Spc的評(píng)估,而非直接訪問(wèn)S[11,25],如公式(8)所示:

(8)

為了避免構(gòu)造矩陣存儲(chǔ)代價(jià)很大的中間矩陣W、WT以及Uλ,可以將公式展開(kāi)后再使用共軛梯度下降算法,這種方式只需存儲(chǔ)算法初始化矩陣而不用再內(nèi)存中存儲(chǔ)矩陣J,因此更適應(yīng)平行化的矩陣和向量操作,更適應(yīng)迭代求解算法需求,公式(8)展開(kāi)后如公式(9)所示:

(9)

預(yù)處理共軛梯度下降(PCG)求解單元如圖5所示,包括了矩陣乘法(虛線框表示)、向量點(diǎn)乘(雙實(shí)線框表示)、向量axpy運(yùn)算(加粗實(shí)線框表示)以及標(biāo)量運(yùn)算(單實(shí)線框表示)4種運(yùn)算.預(yù)處理共軛梯度下降矩陣求解方法根據(jù)計(jì)算量的大小和數(shù)據(jù)的依賴(lài)性,拆分為3個(gè)計(jì)算階段以平衡計(jì)算延遲,增加計(jì)算速度.在使用該單元求解前,首先根據(jù)算法,對(duì)各部分?jǐn)?shù)據(jù)如算法1的第1行進(jìn)行初始化,其中矩陣A=Sp,矩陣b=bschur,矩陣M=DTD;第1個(gè)階段計(jì)算算法行8、行9、行10;第2個(gè)步驟計(jì)算算法行5和行4步驟,計(jì)算分為兩個(gè)部分并行進(jìn)行;第3階段計(jì)算算法中行6、行7,計(jì)算同樣可以分為兩個(gè)部分并行進(jìn)行.每次迭代產(chǎn)生的矩陣x、r、w、p將作為待更新數(shù)據(jù)參與下一輪迭代,直至達(dá)到算法行2以及行3所滿(mǎn)足的退出條件,最終得到優(yōu)化后的矩陣x.自此該單元完成了預(yù)處理共軛梯度下降矩陣求解,得到優(yōu)化后的矩陣x,即相機(jī)參數(shù)的優(yōu)化矩陣.

圖5 預(yù)處理共軛梯度下降法求解器單元硬件架構(gòu)Fig.5 Hardware architecture of PCG for bundle adjustment

預(yù)處理共軛梯度下降求解單元能夠有效進(jìn)行并行加速光束平差法的原因與舒爾構(gòu)造單元相似:1)由于矩陣稀疏性的特點(diǎn)以及矩陣操作允許劃分為塊結(jié)構(gòu)操作的組合,因此可以通過(guò)并行調(diào)用多個(gè)矩陣乘加運(yùn)算單元來(lái)并行加速塊結(jié)構(gòu)操作,進(jìn)而實(shí)現(xiàn)對(duì)該求解單元的并行加速;2)利用算法流程中的數(shù)據(jù)運(yùn)算存在的并行關(guān)系,通過(guò)硬件設(shè)計(jì)提高數(shù)據(jù)運(yùn)算的并行程度來(lái)提高該單元的運(yùn)算速度.

5 實(shí)驗(yàn)結(jié)果與分析

本文使用XilinxTM中的VivadoTM系列的高層次綜合(High Level Sythesis)將C/C++代碼快速綜合生成RTL描述,降低FPGA的開(kāi)發(fā)時(shí)間.評(píng)估數(shù)據(jù)使用Bundle Adjustment in Large(BAL)數(shù)據(jù)集[25]中的部分?jǐn)?shù)據(jù),該數(shù)據(jù)集包含有時(shí)間序列的三維點(diǎn)位置坐標(biāo)、觀測(cè)點(diǎn)坐標(biāo)、相機(jī)位姿參數(shù)等.選用該數(shù)據(jù)集的圖片數(shù)目小于50的數(shù)據(jù)集進(jìn)行測(cè)試,三維點(diǎn)數(shù)目、觀測(cè)點(diǎn)數(shù)目如表1所示.文獻(xiàn)[6,26]中提出所選用規(guī)模的數(shù)據(jù)集足以滿(mǎn)足小型SLAM系統(tǒng)的全局優(yōu)化或者大型SLAM系統(tǒng)的局部?jī)?yōu)化建圖.

表1 測(cè)試所使用的數(shù)據(jù)集Table 1 Datasets used for experiments

本文在不同數(shù)據(jù)集上比較了使用本文方法存儲(chǔ)雅可比矩陣直接獲得連續(xù)的雅可比矩陣以及直接使用BCSR格式存儲(chǔ)進(jìn)行計(jì)算兩種方法之間的時(shí)間性能差異.在5個(gè)數(shù)據(jù)集中分別進(jìn)行10次實(shí)驗(yàn),初始數(shù)據(jù)集添加噪聲后,通過(guò)本文提出的預(yù)處理共軛梯度下降方法求解目標(biāo)方程,表2中的計(jì)算耗時(shí)為每個(gè)數(shù)據(jù)集中迭代計(jì)算100次的平均耗時(shí).可以看出本文提出的改進(jìn)BCSR存儲(chǔ)格式更加有利于連續(xù)雅可比矩陣Jc、JcT、Jp的獲取,因此在計(jì)算Jx、JTe時(shí)間消耗更少,分別提升了約2倍和5倍,并且計(jì)算Jx、JTe耗時(shí)趨于一致,這是由于使用JcT比使用Jc計(jì)算JTe可以提速約40%.

表2 存儲(chǔ)格式計(jì)算時(shí)間比較Table 2 Comparison of time in different storage formats

本文在不同數(shù)據(jù)集上運(yùn)用本文所提的方法求解優(yōu)化方程的方法,即首先進(jìn)行舒爾補(bǔ)構(gòu)造,再運(yùn)用預(yù)處理共軛梯度下降法迭代求解方程.表3為各個(gè)數(shù)據(jù)集上執(zhí)行一次光束平差法時(shí)的各類(lèi)操作類(lèi)型的執(zhí)行時(shí)間占比.通過(guò)表3也可以看出,計(jì)算矩陣乘Jx、JTe兩個(gè)關(guān)鍵步驟,各自約占總耗時(shí)的36%,在硬件架構(gòu)采用脈動(dòng)陣列方式進(jìn)行并行矩陣乘法將有效加速矩陣乘加這一過(guò)程.

表3 BA中各類(lèi)操作時(shí)間比較Table 3 Time comparison of operations in bundle adjustment

本文使用Xilinx Virtex7系列的xc7vx485t-ffg1157-1 FPGA芯片對(duì)本文提出的面向光束平差法的視覺(jué)SLAM并行計(jì)算架構(gòu)設(shè)計(jì)進(jìn)行了評(píng)估,硬件設(shè)計(jì)使用單浮點(diǎn)精度數(shù),綜合得到的時(shí)鐘頻率為346MHz.本文評(píng)估了所提出的框架在CPU計(jì)算平臺(tái)、ARM計(jì)算平臺(tái)與FPGA計(jì)算平臺(tái)在本文BAL測(cè)試數(shù)據(jù)集中運(yùn)行后端優(yōu)化線程的執(zhí)行時(shí)間以及能耗的對(duì)比,結(jié)果如表4所示.CPU計(jì)算平臺(tái)使用Intel(R) Core(TM) i7-4702HQ@2.20GHz型號(hào)的CPU,功率為37W;ARM計(jì)算平臺(tái)選用ARM Cortex-A9@667MHz型號(hào)的處理器,功耗不足2W;而選用的FPGA平臺(tái)的綜合的時(shí)鐘頻率為346MHz,功率約為2W.表4可以看出,在5個(gè)數(shù)據(jù)集中執(zhí)行一次光束平差法的平均執(zhí)行時(shí)間分別為11.92s、190.16s以及6.85s,采用本文提出的并行加速架構(gòu)的FPGA平臺(tái)達(dá)到了最佳的性能,相比Intel x86計(jì)算平臺(tái)有著1.74倍的加速效果提升,而相比ARM計(jì)算平臺(tái)更是達(dá)到了27.76倍的加速效果.同時(shí),本文實(shí)現(xiàn)的FPGA硬件設(shè)計(jì)相比其它兩種計(jì)算平臺(tái)有著突出的能耗優(yōu)勢(shì),平均能量消耗低至約14.06J,該能耗相比Intel x86計(jì)算平臺(tái)和ARM計(jì)算平臺(tái)分別節(jié)省了96.6%和95.0%的能量消耗.

表4 3種計(jì)算平臺(tái)光束平差法執(zhí)行時(shí)間與能耗對(duì)比Table 4 Comparison of execution time and energy consumption of bundle adjustment on three computing platforms

表5列出了所提出的面向光束平差法的視覺(jué)SLAM并行計(jì)算架構(gòu)的硬件資源使用情況,本文提出的加速架構(gòu)占用FPGA芯片中35%的隨機(jī)存取存儲(chǔ)器(Block RAM, BRAM)、6%的數(shù)字信號(hào)處理器(Digital Signal Processor, DSP)、74%的觸發(fā)器(Flip Flop, FF)以及52%的查找表(Look Up Table, LUT),FPGA中仍有大量的資源可以用于繼續(xù)拓展和實(shí)現(xiàn)一些計(jì)算量大的其他任務(wù).

表5 資源匯總Table 5 FPGA resource summary

本文在5個(gè)數(shù)據(jù)集上對(duì)比了在Intel x86平臺(tái)、ARM平臺(tái)和本文提出的FPGA硬件架構(gòu)在使用光束平差法的優(yōu)化效果,表6可以看出,3個(gè)計(jì)算平臺(tái)中優(yōu)化后總累計(jì)誤差相同,說(shuō)明本文架構(gòu)取得了相同的優(yōu)化效果.通過(guò)表5和表6,表明在SLAM后端優(yōu)化問(wèn)題中,文本架構(gòu)在取得速度提升和能耗降低的優(yōu)勢(shì)下依然能夠達(dá)到與Intel x86、AMR平臺(tái)中運(yùn)行光束平差法同樣的優(yōu)化結(jié)果.

表6 3種計(jì)算平臺(tái)光束平差法優(yōu)化效果對(duì)比Table 6 Comparison of bundle adjustment results on three computing platforms

最后,本文在所提出框架的FPGA平臺(tái)、Intel x86平臺(tái)以及ARM平臺(tái)上驗(yàn)證了運(yùn)行主流視覺(jué)SLAM算法ORB SLAM的性能效果,實(shí)驗(yàn)選取KITTI數(shù)據(jù)集[27]中的第11～21個(gè)序列作為驗(yàn)證數(shù)據(jù)集.表7展示了3個(gè)平臺(tái)中運(yùn)行算法分別在200米、400米、600米、800米路徑長(zhǎng)度下的累計(jì)平移誤差和累計(jì)旋轉(zhuǎn)誤差的對(duì)比結(jié)果,可以看出本文FPGA在累計(jì)平移誤差和累計(jì)旋轉(zhuǎn)誤差中都取得了同樣的優(yōu)化性能,經(jīng)過(guò)800m后的累計(jì)平移誤差為6.66m,累計(jì)旋轉(zhuǎn)誤差為0.184°.

表7 3種計(jì)算平臺(tái)ORB SLAM算法結(jié)果對(duì)比Table 7 Comparison of ORB SLAM results on three computing platform

本文將算法生成的軌跡與數(shù)據(jù)集中的實(shí)際軌跡進(jìn)行可視化對(duì)比,圖6展示了在第13個(gè)數(shù)據(jù)集序列中的關(guān)鍵點(diǎn)軌跡圖,可以看出在FPGA中運(yùn)行的SLAM算法僅出現(xiàn)很小的軌跡偏移,達(dá)到了很好的視覺(jué)SLAM優(yōu)化性能.

圖6 FPGA平臺(tái)ORB SLAM算法軌跡圖Fig.6 ORB-SLAM trajectories in sequence 13 on FPGA

ORB SLAM算法分為追蹤、局部建圖、回環(huán)檢測(cè)3個(gè)線程執(zhí)行,而地圖構(gòu)建部分中因執(zhí)行局部光束平差法而最為耗時(shí).本文統(tǒng)計(jì)了3個(gè)平臺(tái)運(yùn)行ORB SLAM算法時(shí)在局部建圖線程總耗時(shí)以及光束平差法的耗時(shí)情況,在KITTI數(shù)據(jù)集的第11～21個(gè)序列中,3平臺(tái)的平均總耗時(shí)如表8所示,可以看出本文所提框架實(shí)現(xiàn)的FPGA有效加速了光束平差法部分,光束平差法執(zhí)行時(shí)間為73.57ms,執(zhí)行速率接近13.6fps,優(yōu)于x86的執(zhí)行速度.在整個(gè)局部建圖線程中,盡管FPGA中的耗時(shí)比Intel x86的耗時(shí)要長(zhǎng),但FPGA更適用于機(jī)器人的嵌入式平臺(tái),且能耗節(jié)省了約83%,因此也有一定的意義.此外,對(duì)比ARM和FPGA計(jì)算平臺(tái)執(zhí)行時(shí)間與能耗可以發(fā)現(xiàn),本文框架的FPGA相比ARM平臺(tái)的局部建圖執(zhí)行速度有著超過(guò)5倍的提升,同時(shí)節(jié)省了約80%的能量消耗.

表8 3種計(jì)算平臺(tái)在局部建圖線程中的時(shí)間與能耗對(duì)比Table 8 Comparison of the time and energy consumption of local mapping thread on three computing platforms

因此,本文FPGA框架在機(jī)器人嵌入式平臺(tái)開(kāi)發(fā)中能夠在較低的能耗水平下有效地通過(guò)加速光束平差法來(lái)加速ORB SLAM執(zhí)行速度,同時(shí)有著很好的視覺(jué)SLAM優(yōu)化效果.

6 總 結(jié)

為解決視覺(jué)SLAM后端優(yōu)化對(duì)算力需求巨大的問(wèn)題,本文提出了一種視覺(jué)SLAM后端優(yōu)化部分硬件加速的光速平差法加速器硬件架構(gòu),通過(guò)軟硬件結(jié)合的方式利用高層次綜合方法進(jìn)行C++高級(jí)語(yǔ)言的綜合,設(shè)計(jì)并實(shí)現(xiàn)了面向光束平差法的視覺(jué)SLAM并行計(jì)算架構(gòu),該架構(gòu)采用的改進(jìn)BCSR存儲(chǔ)方式更有利于矩陣存取,并構(gòu)建出舒爾補(bǔ)構(gòu)造單元和PCG求解大規(guī)模正定矩陣方程的加速單元用來(lái)簡(jiǎn)化和加速關(guān)鍵步驟.由于光束平差法被廣泛地應(yīng)用在SLAM后端優(yōu)化環(huán)節(jié),該架構(gòu)能夠靈活適用于多種SLAM算法的后端優(yōu)化環(huán)節(jié)的運(yùn)算,具有配置靈活、運(yùn)算速度快、功耗低等特點(diǎn).