面向光束平差法的视觉SLAM并行计算架构与实现

成 祥,陈迟晓,翟 鹏,张立华

1(复旦大学 智能机器人研究院,上海 200433)

2(上海智能机器人工程技术研究中心,上海 200433)

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

4(季华实验室,广东 佛山 528000)

5(吉林省人工智能与无人系统工程研究中心,长春 130000)

E-mail:lihuazhang@fudan.edu.cn

自1980年以来,同步定位和建图(Simultaneous Localization and Mapping,SLAM)一直是计算机视觉和机器人技术领域的活跃研究领域.SLAM为机器人提供了一种用户创建地图的替代方案,允许机器人在没有预先获得场景的本地化基础结构的情况下,进行机器人的定位和建图.经典的视觉SLAM系统的体系结构包括前端视觉里程计、后端优化、回环检测以及建图4个组件,其中后端优化对SLAM系统的定位精度和建图质量有着重要的作用[1-3].

后端优化对于整个视觉SLAM系统尤为重要,优化求解的速度与求解能力直接影响到视觉SLAM的实时性及准确性,然而实际应用中,视觉SLAM后端优化环节需要大量的计算资源,耗费大量的计算时间.近些年来,一方面,视觉SLAM算法执行后端优化步骤的频率受到了硬件设备算力的限制,开发者不得不通过降低后端优化的频率,从而在实时性与性能之间寻找平衡点,然而这在一定程度上影响了SLAM算法的工作性能;另一方面,视觉SLAM算法对计算资源的高要求使得部署视觉SLAM算法的机器人对硬件设备要求很高,造成机器人设计和应用的成本大幅提高.现有技术实现SLAM算法通常使用通用处理器(CPU)或图形处理器(GPU)进行计算,通用性差、效率较低且成本较高.

针对上述问题,本文提出了针对视觉SLAM后端优化的一种通用加速架构,实现了基于光束平差法后端优化部分的硬件运算加速,能够灵活适用于各种视觉SLAM算法的后端优化部分的运算,进而可以减少运算时间,从而有望降低机器人设计和应用的门槛,提升视觉SLAM系统性能,拓展机器人的应用空间.

当前,SLAM算法的算力需求与处理器运算能力之间的矛盾日益显著.在过去的几十年间,SLAM算法因其计算复杂度和实时性要求只能在高性能计算机上执行.随着摩尔定律的发展到达瓶颈期,通用嵌入式处理器的运算能力已达到饱和,约为5GOP/s[4](1)https://zenodo.org/record/3947824#.YA6Gq6j7SUk,然而快速发展的SLAM新算法对硬件平台的算力需求已从5GOP/s上升到50GOP/s,SLAM算法与深度学习/语义理解结合,运算能力的要求将达到500GOP/s.其中SLAM后端运算占据总运算需求的50%[5-9].

研究者对SLAM加速问题进行了长久的探索.Jeong等人[10]利用SLAM问题的稀疏特性提出了简化相机模型系统,并基于块稀疏矩阵加速的特性加速了SLAM的处理速度.Wu[11]等人提出了针对CPU和GPU的多核加速方案,通过预处理共轭梯度下降法优化矩阵向量乘积减少了内存大小,并且该方法更有利于并行化运算.Eriksson等人[12]以及Zhang等人[13]提出了针对大规模光束平差法的分布式计算方法,并通过分割相机共视地图点的方法减少了分布式计算的通信开销.这些探索都是在通用处理器或图形处理器上开展,尽管有一定发展,但没有很好的利用到光束平差法在优化内存和计算资源以及加快运算速度方面的优势.

此外,一些研究者在硬件加速器方面尝试加速SLAM算法.eSLAM[14]提出了一种在FPGA平台上加速特征提取和匹配阶段的实时ORB SLAM的节能架构,从而实现了在低功耗平台上实现实时SLAM算法.Xu等人[15]提出了基于CNN的特征点提取的SLAM加速器来加速前端特征提取速度.Schulz等人[16]基于ARM的嵌入式FPGA SoC实现了用于视觉SLAM的立体视觉预处理模块.上述文献[14-16]中的SLAM加速器仅仅对前端进行加速,而没有考虑运算需求更大的后端运算.一些研究者对特定的SLAM算法在嵌入式计算平台进行实现,如Sugiura[17]针对2D LiDAR的GMapping算法提出了一种并行加速扫描与匹配的加速器,Sileshi等人[18]以及Abouzahir等人[19]实现了基于粒子过滤器的Fast SLAM的FPGA加速器Tertei等[20]人以及Hanif等人[21]实现了针对3D Visual SLAM的EKF-SLAM加速器块,Gautier等人[19]针对视觉、激光融合SLAM算法框架InfiniTAM提出了加速深度融合和射线投射模块的FPGA架构.密歇根大学提出了一个加速半全局匹配过程的并行处理器[22],能够实现密集的实时3D深度和3D运动感知的摆姿势设计可在全高清(1920×1080,FHD)分辨率下实现基于相邻像素的半全局匹配,只加速SLAM运算量较小的姿态估计部分.特文特大学使用FPGA设计了GraphSLAM的并行化系数求解架构[23],可以执行共轭梯度算法和稀疏矩阵的点积、加法、缩放操作,但是无法实现对运算需求更大、操作更频繁的矩阵乘法的运算,同时框架不能支持SLAM后端优化中更普遍使用的光束平差法.

总之,现有技术缺乏有效的、灵活且通用的SLAM算法后端优化部分加速方法和装置,尚未存在一款用于后端优化流程的可以灵活配置的专用SLAM硬件加速结构.

3.1 相机模型

在计算机视觉领域,相机模型通常利用几何知识和笛卡尔坐标系建模,通过小孔成像原理,将理想成像过程转化成通过3个方向上的平移和旋转实现,最终将现实世界的三维空间点投影到二维图像像素点.当相机模型建模用齐次坐标形式进行描述,可以表述为公式(1):

(1)

其中,K为相机内参数矩阵,T∈4×4为相机位姿变换矩阵,T可以分解成旋转矩阵R∈3×3和位移矩阵t=[t1,t2,t3]T两部分构成,Pw=[X,Y,Z]T为当前世界坐标中的点,Puv=[u,v,1]T为图像坐标系中的点,Z为世界坐标系中的z轴坐标.

3.2 光束平差法

视觉SLAM中,在将估计产生的轨迹和地图带入到SLAM的运动和观测模型中时,传感器误差、里程计积累误差以及特征点之间的误差等误差会导致模型不完美,随着时间的积累,甚至可能导致SLAM模型中的轨迹发生错误、地图严重失准等问题.解决这一问题的一种有效手段是,通过局部优化或全局优化来微调模型参数,不断缩小视觉SLAM系统误差达到一个在允许范围内的极小值,从而使视觉SLAM模型在长时间的运行过程中依旧保持鲁棒性和准确性.局部优化或全局优化问题通常被表述为非线性最小二乘问题,误差定义为观测到的特征位置与相应3D点在相机图像平面上的投影位置之间差值的平方,优化方程如公式(2)所示:

(2)

其中,x为待优化参数的向量,由相机位姿向量xc∈SE(3)和特征点参数向量xp∈3组成,即是3D重建的重投影误差的向量,x*是优化求解后的最优参数向量.

光束平差法是求解视觉SLAM后端非线性优化问题的一个经典方法,它能够精确的优化每一个相机位姿以及每一个地图特征点的位置.光束平差法的算法基本思想是,假设在一次观测中,场景中的所有观测点都会反射出光束,算法通过优化实际观测点和估计点之间的重投影误差调整相机位姿以及地图特征点位置,使得所有反射产生的光束最终都可以交汇于一点,即所有光束最终都可以由相机发出,进而构建视觉SLAM后端优化模型.

3.3 Levenberg-Marquardt方法

迭代求解算法Levenberg-Marquardt(LM)[1-3]对于解决非线性最小二乘问题有着良好的性能,它结合了高斯牛顿迭代法和最速下降法,并采用二阶泰勒展开近似,同时考虑使用置信区域来判断泰勒近似的程度,其求解公式如公式(3)所示:

(3)

其中,J(x)为f(x)的雅可比矩阵,D(x)是非负对角矩阵,由对角线矩阵J(x)TJ(x)的平方根计算所得,λ为控制正则化强度的拉格朗日乘子.如果‖f(x+δ*)‖<‖f(x)‖,则更新x←x+δ*,通过λ控制更新量的大小.当λ较大时,公式近似为最速下降法,当λ较小时,公式近似为高斯牛顿迭代法.参数λ依据雅可比矩阵J(x)逼近f(x)的程度来更新.相比高斯牛顿法,LM算法的求解方式能够提供更稳定更准确的增量值δ*.

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

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

(4)

光束平差法的优化方程,可以重写为矩阵形式.记Hλ=JTJ+λDTD,Hλ为N×N的矩阵,N=6n+3m.令U=JcT,V=JpTJp,Uλ=U+λDcTDc,Vλ=V+λDpTDp,W=JcTJp,则公式(4)可以重写为公式(5):

(5)

其中,Uλ是对角块矩阵,每个块为6×6的矩阵,只与相机姿态有关,对角线共有n个小矩阵块;Vλ同样是对角块矩阵,每个块为3×3的矩阵,只与地图特征点有关,对角线共有m个小矩阵块;矩阵W与WT矩阵互为转置,其稀疏情况由具体的相机位姿所观测到的数据决定.

3.4 舒尔消解

在SLAM后端优化步骤中,相机位姿参数的个数n一般维护在不超过100,而地图点的数量m能够达到10000以上,即使使用关键帧以及稀疏性的特征,每个关键帧通常能够观测到超过2000个关键点,即n≪m.这就导致公式(5)中的矩阵规模很大,在求解运算过程中耗费大量算力.

公式(5)具有稀疏化的特性,这是由雅可比矩阵的稀疏性所决定的.光束平差法的关键特征是,待优化函数误差项仅与观测相机位姿所观测得到的地图特征点位置有关——当仅考虑涉及到的第i个相机以及其可以观测到的第j个地图点特征点时,雅可比矩阵在只涉及第i个相机参数和第j个路标点的项中不为零,其余项全部为零.公式(6)描述了上述雅可比矩阵特性:

(6)

由于雅可比矩阵的稀疏化的特性,公式(5)第1项存在大量的零矩阵块,其中矩阵块Uλ和矩阵块Vλ除对角线分别存在关于相机位姿的6×6的矩阵块和关于地图点特征点的3×3的矩阵块外,其余均为零矩阵块;矩阵块W以及其转置WT也同样存在大量零矩阵块,只有在涉及第i个相机参数和第j个路标点的项存在非零数据,可以将非对角线上的非零矩阵块形象地当作是变量之间存在的约束条件.

(7)

4.1 SLAM后端专用并行化计算架构

本文提出一种面向光束平差法的视觉SLAM并行计算硬件架构,通过加速后端优化部分,能有效加速采用了光束平差法的各种视觉SLAM算法,如图1所示.FPGA包含处理系统(Processing System, PS)和可编程系统(Programmable Logic, PL)两部分,PL部分包括用于加速SLAM中的矩阵乘加运算单元、舒尔补构造单元以及预处理共轭梯度下降法求解器单元,PL部分的3类单元分别用于矩阵乘加操作、舒尔消解过程与正定矩阵方程求解过程,PS部分执行基于光束平差法的视觉SLAM后端优化部分其他步骤.

图1 光束平差法加速器硬件架构Fig.1 Architecture of bundle adjustment accelerator

PS部分与PS部分之间通过总线通信,由于SLAM后端数据流复杂,因此不同模块分时复用,并采用流水线设计方式,由通用处理器进行控制和调度,其中包括以下4个步骤:

步骤1.预计算Hλ的相关矩阵,此步骤将由矩阵乘加运算单元实际计算和加速;

步骤2.通过专用舒尔补构造加速单元并行地构造舒尔补矩阵S、bschur;

步骤3.利用预处理共轭梯度方法的专用矩阵迭代计算加速单元迭代求解矩阵方程,获得相机位姿参数的改变量δc;

步骤4.计算地图点空间改变量δp,此步骤也将由矩阵乘加运算单元参与加速.

自此,SLAM系统完成了一个后端优化步骤,更新了相机位姿参数以及地图点信息.图2形象地说明了SLAM后端优化的4个步骤中具体硬件与操作的映射关系.

图2 硬件与操作的映射关系Fig.2 Hardware mapping for bundle adjustment ccelerator

4.2 数据存储方式

在SLAM后端优化求解过程中,直接存储原始数据、各类矩阵、中间结果等数据将消耗巨大的内存空间,且影响数据存取及运算时间,所以影响性能的一个关键设计决策是如何将各种矩阵存储在RAM中.

加速光束平差加速器硬件架构的PL部分输入为投影误差e=[ec;ep]、相机参数雅可比矩阵Jc以及地图点参数雅可比矩阵Jp等矩阵,经过运算输出相机位姿改变量矩阵δc以及地图特征点改变量矩阵δp.考虑到存在的稀疏的、块结构的矩阵特性,可以利用块结构压缩行稀疏(Block Compressed Sparse Row,BCSR)的方式存储稀疏的矩阵,该方法能减少存储空间,有利于并行化运算.

典型的BCSR格式使用3个属性:按行顺序线性存储的非零块的值vals,所对应位置原始非零块的列索引cols,以及每行的第1个非零块的在vals阵列中的索引row_blk.在光束平差法中,使用BCSR格式存储时,vals按行顺序线性存储非零元素块,cols表示相机参数雅可比矩阵Jc以及地图点参数雅可比矩阵Jp的非零块所在列索引.本文与BCSR不同的一点是,使用共视集合数组(co-visibility set,COi)代替row_blk,共视集合的物理意义为所能观测到第i个点的图像集合,例如在图3中,CO4={2,3}即为从图像2和图像3中可以观测到地图点4,集合元素的数目可以作为row_blk记录每行非零元素起始位置.该改进方式包含光束平差法问题的物理含义的指针数组COi,可以直接用于后续计算中,而无需进一步转换,有利于在硬件中进行并行化计算.

图3 改进的BCSR数据存储格式Fig.3 Improved BCSR data storage format

算法产生的中间结果舒尔补矩阵S为稠密的大规模对称正定矩阵,本文考虑将该矩阵的分为对角线上的对角块、对角线以上以及对角线以下的对称矩阵部分,如图3右上角图片所示,本文在设计硬件存储中仅需存储舒尔补矩阵S对角线上以及对角线以上的部分,这将节省近一半的存储空间.

4.3 舒尔补构造单元

为了解决求解方程规模过大的问题,本文尝试利用光束平差法中Hλ的稀疏块结构的特性设计专用舒尔补构造单元,如图4所示.由于存在舒尔消解的方式,可以将规模为N×N(N=3m+6n)的方阵简化到规模为6n的方阵,且通常n≪m,这将极大简化和加速矩阵方程求解过程.

图4 舒尔补构造单元硬件架构Fig.4 Hardware architecture of schur elimination

舒尔补构造单元硬件架构依照块结构的舒尔消解方法进行舒尔补矩阵构造,该舒尔补构造单元的输入包括投影误差e、相机参数雅可比矩阵Jc以及地图点参数雅可比矩阵Jp等矩阵,依据计算的数据依赖性可以将计算过程分为5个部分,共计12个计算阶段,同时几种不同的部分根据计算量的大小和数据的依赖性,拆分为5个计算阶段以平衡计算延迟,增加计算速度.舒尔补构造单元硬件架构如图4所示.图中虚线框部分为专用的矩阵乘加单元.第1个部分计算地图点的雅可比矩阵Jp的转置与其本身的矩阵乘加V=∑JpTJp,以及计算其矩阵的逆V-1;第2个部分计算地图点的雅可比矩阵Jp和投影误差e的矩阵乘p,之后依次与矩阵V-1、Jp和Jc进行矩阵乘加运算,得到JcJpV-1p;第3个部分计算关于相机参数的雅可比矩阵Jc和投影误差e的矩阵乘c,与第1部分结果相减得到bschur=c-∑JcTJpV-1p;第4个部分计算相机参数的雅可比矩阵Jp的转置与地图点的雅可比矩阵的转置JpT与的矩阵乘加运算WT=∑JpTJc,计算其转置后,之后依次与矩阵V-1、Jp和Jc进行矩阵乘加运算,得到JcTJpV-1WT;第5个部分计算相机参数的雅可比矩阵的转置JcT与其本身的矩阵乘加U=∑JcTJc,之后计算与第4部分计算结果的矩阵减法,得到矩阵Hλ的舒尔补矩阵S=U-∑JcTJpV-1WT.自此,该单元完成了舒尔补消解后的求解方程的构造.

舒尔补构造单元能够有效地并行加速的原因有两个:1)由于雅可比矩阵及海森矩阵具有稀疏的特性,构造过程中矩阵可以按照块结构划分,将块结构作为运算的基本单元,矩阵稀疏特性允许只需在涉及到对应部分的块结构上进行矩阵操作,因此可以并行地调用多个采用并行加速设计的矩阵乘加运算单元实现按块构造;2)该单元将舒尔构造流程中无数据依赖的可并行的过程进行并行设计,例如图4中不同计算阶段下的操作可以并行进行,这也是舒尔补构造单元能够进行并行加速的一个原因.

4.4 预处理共轭梯度下降法求解单元

预处理共轭梯度下降法是一种可以通过迭代的方式逼近矩阵方程解的求解方法,这种迭代求解允许在计算过程中不直接存储矩阵A,而是使用Ap代替,能够良好的处理大规模矩阵方程,收敛性取决于所有特征值的分布情况[24].预处理共轭梯度下降求解算法如算法1所示.

算法 1.预处理共轭梯度下降法求解矩阵方程

输入：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

使用预处理矩阵技术,在舒尔补上进行共轭梯度下降算法,利用舒尔补构造实现对Spc的评估,而非直接访问S[11,25],如公式(8)所示:

(8)

为了避免构造矩阵存储代价很大的中间矩阵W、WT以及Uλ,可以将公式展开后再使用共轭梯度下降算法,这种方式只需存储算法初始化矩阵而不用再内存中存储矩阵J,因此更适应平行化的矩阵和向量操作,更适应迭代求解算法需求,公式(8)展开后如公式(9)所示:

(9)

预处理共轭梯度下降(PCG)求解单元如图5所示,包括了矩阵乘法(虚线框表示)、向量点乘(双实线框表示)、向量axpy运算(加粗实线框表示)以及标量运算(单实线框表示)4种运算.预处理共轭梯度下降矩阵求解方法根据计算量的大小和数据的依赖性,拆分为3个计算阶段以平衡计算延迟,增加计算速度.在使用该单元求解前,首先根据算法,对各部分数据如算法1的第1行进行初始化,其中矩阵A=Sp,矩阵b=bschur,矩阵M=DTD;第1个阶段计算算法行8、行9、行10;第2个步骤计算算法行5和行4步骤,计算分为两个部分并行进行;第3阶段计算算法中行6、行7,计算同样可以分为两个部分并行进行.每次迭代产生的矩阵x、r、w、p将作为待更新数据参与下一轮迭代,直至达到算法行2以及行3所满足的退出条件,最终得到优化后的矩阵x.自此该单元完成了预处理共轭梯度下降矩阵求解,得到优化后的矩阵x,即相机参数的优化矩阵.

图5 预处理共轭梯度下降法求解器单元硬件架构Fig.5 Hardware architecture of PCG for bundle adjustment

预处理共轭梯度下降求解单元能够有效进行并行加速光束平差法的原因与舒尔构造单元相似:1)由于矩阵稀疏性的特点以及矩阵操作允许划分为块结构操作的组合,因此可以通过并行调用多个矩阵乘加运算单元来并行加速块结构操作,进而实现对该求解单元的并行加速;2)利用算法流程中的数据运算存在的并行关系,通过硬件设计提高数据运算的并行程度来提高该单元的运算速度.

本文使用XilinxTM中的VivadoTM系列的高层次综合(High Level Sythesis)将C/C++代码快速综合生成RTL描述,降低FPGA的开发时间.评估数据使用Bundle Adjustment in Large(BAL)数据集[25]中的部分数据,该数据集包含有时间序列的三维点位置坐标、观测点坐标、相机位姿参数等.选用该数据集的图片数目小于50的数据集进行测试,三维点数目、观测点数目如表1所示.文献[6,26]中提出所选用规模的数据集足以满足小型SLAM系统的全局优化或者大型SLAM系统的局部优化建图.

表1 测试所使用的数据集Table 1 Datasets used for experiments

本文在不同数据集上比较了使用本文方法存储雅可比矩阵直接获得连续的雅可比矩阵以及直接使用BCSR格式存储进行计算两种方法之间的时间性能差异.在5个数据集中分别进行10次实验,初始数据集添加噪声后,通过本文提出的预处理共轭梯度下降方法求解目标方程,表2中的计算耗时为每个数据集中迭代计算100次的平均耗时.可以看出本文提出的改进BCSR存储格式更加有利于连续雅可比矩阵Jc、JcT、Jp的获取,因此在计算Jx、JTe时间消耗更少,分别提升了约2倍和5倍,并且计算Jx、JTe耗时趋于一致,这是由于使用JcT比使用Jc计算JTe可以提速约40%.

表2 存储格式计算时间比较Table 2 Comparison of time in different storage formats

本文在不同数据集上运用本文所提的方法求解优化方程的方法,即首先进行舒尔补构造,再运用预处理共轭梯度下降法迭代求解方程.表3为各个数据集上执行一次光束平差法时的各类操作类型的执行时间占比.通过表3也可以看出,计算矩阵乘Jx、JTe两个关键步骤,各自约占总耗时的36%,在硬件架构采用脉动阵列方式进行并行矩阵乘法将有效加速矩阵乘加这一过程.

表3 BA中各类操作时间比较Table 3 Time comparison of operations in bundle adjustment

本文使用Xilinx Virtex7系列的xc7vx485t-ffg1157-1 FPGA芯片对本文提出的面向光束平差法的视觉SLAM并行计算架构设计进行了评估,硬件设计使用单浮点精度数,综合得到的时钟频率为346MHz.本文评估了所提出的框架在CPU计算平台、ARM计算平台与FPGA计算平台在本文BAL测试数据集中运行后端优化线程的执行时间以及能耗的对比,结果如表4所示.CPU计算平台使用Intel(R) Core(TM) i7-4702HQ@2.20GHz型号的CPU,功率为37W;ARM计算平台选用ARM Cortex-A9@667MHz型号的处理器,功耗不足2W;而选用的FPGA平台的综合的时钟频率为346MHz,功率约为2W.表4可以看出,在5个数据集中执行一次光束平差法的平均执行时间分别为11.92s、190.16s以及6.85s,采用本文提出的并行加速架构的FPGA平台达到了最佳的性能,相比Intel x86计算平台有着1.74倍的加速效果提升,而相比ARM计算平台更是达到了27.76倍的加速效果.同时,本文实现的FPGA硬件设计相比其它两种计算平台有着突出的能耗优势,平均能量消耗低至约14.06J,该能耗相比Intel x86计算平台和ARM计算平台分别节省了96.6%和95.0%的能量消耗.

表4 3种计算平台光束平差法执行时间与能耗对比Table 4 Comparison of execution time and energy consumption of bundle adjustment on three computing platforms

表5列出了所提出的面向光束平差法的视觉SLAM并行计算架构的硬件资源使用情况,本文提出的加速架构占用FPGA芯片中35%的随机存取存储器(Block RAM, BRAM)、6%的数字信号处理器(Digital Signal Processor, DSP)、74%的触发器(Flip Flop, FF)以及52%的查找表(Look Up Table, LUT),FPGA中仍有大量的资源可以用于继续拓展和实现一些计算量大的其他任务.

表5 资源汇总Table 5 FPGA resource summary

本文在5个数据集上对比了在Intel x86平台、ARM平台和本文提出的FPGA硬件架构在使用光束平差法的优化效果,表6可以看出,3个计算平台中优化后总累计误差相同,说明本文架构取得了相同的优化效果.通过表5和表6,表明在SLAM后端优化问题中,文本架构在取得速度提升和能耗降低的优势下依然能够达到与Intel x86、AMR平台中运行光束平差法同样的优化结果.

表6 3种计算平台光束平差法优化效果对比Table 6 Comparison of bundle adjustment results on three computing platforms

最后,本文在所提出框架的FPGA平台、Intel x86平台以及ARM平台上验证了运行主流视觉SLAM算法ORB SLAM的性能效果,实验选取KITTI数据集[27]中的第11～21个序列作为验证数据集.表7展示了3个平台中运行算法分别在200米、400米、600米、800米路径长度下的累计平移误差和累计旋转误差的对比结果,可以看出本文FPGA在累计平移误差和累计旋转误差中都取得了同样的优化性能,经过800m后的累计平移误差为6.66m,累计旋转误差为0.184°.

表7 3种计算平台ORB SLAM算法结果对比Table 7 Comparison of ORB SLAM results on three computing platform

本文将算法生成的轨迹与数据集中的实际轨迹进行可视化对比,图6展示了在第13个数据集序列中的关键点轨迹图,可以看出在FPGA中运行的SLAM算法仅出现很小的轨迹偏移,达到了很好的视觉SLAM优化性能.

图6 FPGA平台ORB SLAM算法轨迹图Fig.6 ORB-SLAM trajectories in sequence 13 on FPGA

ORB SLAM算法分为追踪、局部建图、回环检测3个线程执行,而地图构建部分中因执行局部光束平差法而最为耗时.本文统计了3个平台运行ORB SLAM算法时在局部建图线程总耗时以及光束平差法的耗时情况,在KITTI数据集的第11～21个序列中,3平台的平均总耗时如表8所示,可以看出本文所提框架实现的FPGA有效加速了光束平差法部分,光束平差法执行时间为73.57ms,执行速率接近13.6fps,优于x86的执行速度.在整个局部建图线程中,尽管FPGA中的耗时比Intel x86的耗时要长,但FPGA更适用于机器人的嵌入式平台,且能耗节省了约83%,因此也有一定的意义.此外,对比ARM和FPGA计算平台执行时间与能耗可以发现,本文框架的FPGA相比ARM平台的局部建图执行速度有着超过5倍的提升,同时节省了约80%的能量消耗.

表8 3种计算平台在局部建图线程中的时间与能耗对比Table 8 Comparison of the time and energy consumption of local mapping thread on three computing platforms

因此,本文FPGA框架在机器人嵌入式平台开发中能够在较低的能耗水平下有效地通过加速光束平差法来加速ORB SLAM执行速度,同时有着很好的视觉SLAM优化效果.

为解决视觉SLAM后端优化对算力需求巨大的问题,本文提出了一种视觉SLAM后端优化部分硬件加速的光速平差法加速器硬件架构,通过软硬件结合的方式利用高层次综合方法进行C++高级语言的综合,设计并实现了面向光束平差法的视觉SLAM并行计算架构,该架构采用的改进BCSR存储方式更有利于矩阵存取,并构建出舒尔补构造单元和PCG求解大规模正定矩阵方程的加速单元用来简化和加速关键步骤.由于光束平差法被广泛地应用在SLAM后端优化环节,该架构能够灵活适用于多种SLAM算法的后端优化环节的运算,具有配置灵活、运算速度快、功耗低等特点.