一种适用于SLAM非线性并行化芯片计算架构及工作方法

一种适用于slam非线性并行化芯片计算架构及工作方法
技术领域
1.本发明属于计算加速芯片技术领域，尤其涉及一种适用于slam非线性并行化芯片计算架构及工作方法。


背景技术：

2.现有技术通常使用通用处理器实现slam算法，无法满足常用的实时slam运算的需求，该方法通常降低后端优化的频率满足实时性的要求，这就使得后端优化性能大打折扣。实现slam算法的另一种方式是再图形处理器(gpu)中运算，该方法由于slam运算复杂性不能完全支持后续运算，进而无法达到全局有效加速的效果。
3.针对运算能力瓶颈问题，厂商与研究机构在寻求通过硬件设计来加速针对机器人的算法。eslam提出了一种在fpga平台上加速特征提取和匹配阶段的实时orbslam的节能架构，从而实现了在低功耗平台上实现实时slam算法。eslam仅仅对前端进行减速，而更运算需求更大的后端运算没有考虑在内。
4.intel公司提出了一种通用多机器人系统，包含了slam、路径规划等多种功能，可以完成搜索和救援，精确农业和工业自动化等功能。该系统采用通用功能的处理器处理机器人系统的运算，集成了用于传感器数据获取和预处理的主机处理器；用于定位/映射，避免碰撞和协作智能决策的tensilicadsp处理器；专用路径规划和运动控制硬件加速器；用于人类语音检测带有音频加速器；用于对象检测和识别的cnn加速器。没有针对slam专门设计的专用加速器。密歇根大学提出一个加速半全局匹配过程的并行处理器。能够实现密集的实时3-d深度和3-d运动感知的摆姿势设计可在全高清(1920
×
1080，fhd)分辨率下实现邻居引导的半全局匹配，从而实现全高清分辨率下实时无人机自主飞行。但问题加速处理器只加速姿态估计部分，而位姿估计是slam系统的一个很小的模块。
5.此外，现有技术中关于slam相关的专利，如slam运算装置和方法，实现了一种slam硬件加速器的装置，包括存储、运算、控制三大部分，公开了向量运算单元和矩阵运算单元的加速装置，能够有效根据不同的需求对slam算法进行加速，能够适用于各种slam算法和多种不同的输入数据类型，满足不同需求的运算，具有灵活性强、可配置程度高、运算速度快、功耗低等优点。但是其矩阵运算是针对16维方阵设计的，且没有对后端优化做特殊设计，是对slam通用运算的加速。
6.专利名称为一种用于slam的已知自身位姿的光束平差法fpga加速器公开了一种用于已知位姿更新的光束平差法的fpga加速器，实现了旋转矩阵处理器和重投影处理器两个硬件模块，两个模块通过划分计算阶段实现了计算时间的平衡以及中间变量的再利用，从而提高了运算速度、节约了硬件资源。
7.但是该发明建立在位姿已知的场景下，且其加速方法只是光束平差法中的一个步骤，其运算量更大的关键步骤未能包括在内，因此适用性较为局限。


技术实现要素：

8.本发明的目的是为用于后端优化流程的专用slam硬件加速结构解决现有单独加速前端的适应性差问题，而提出的一种适用于slam非线性并行化芯片计算架构及工作方法。
9.为了实现上述目的，本发明采用了如下技术方案：
10.本发明提供了一种适用于slam非线性并行化芯片计算架构，包括至少一个基于脉动阵列的块结构并行化矩阵乘加单元，用于将大规模矩阵分解成最大为6
×
6规模的块结构矩阵并行化的乘加操作；
11.至少一个求解大规模对称正定矩阵方程的预处理共轭梯度方法的迭代求解器；
12.以及硬件映射模块用于slam后端优化中复杂数据流的处理分析。
13.优选地，所述迭代求解器将块结构的舒尔构造的矩阵方程，按照块结构并行运算的方式进行正定矩阵的基于预处理共轭梯度方法的求解，从而获得相机位姿参数的优化改变量，进而获得优化的图点的改变量。
14.优选地，所述迭代求解器以舒尔补矩阵并行化构造加速单元将三维地图点m个以及六维相机位姿n个，对观测投影坐标误差的海森矩阵快速进行舒尔消解，使得大规模矩阵方程降为到6n
×
6n的规模。
15.优选地，地图点数量m大于相机位姿数目n，所述舒尔补矩阵并行化构造加速单元降低运算矩阵的规模，矩阵运算操作将由并行化矩阵运算单元进行运算和加速，且数量由加速器资源决定。
16.本发明提供了一种适用于slam非线性并行化芯片计算架构的工作方法，具体包括如下步骤
17.步骤一：预计算相关矩阵，此步骤将由矩阵运算单元实际计算和加速；
18.步骤二：迭代求解器通过舒尔补构造加速单元进行并行的舒尔补矩阵的构造；
19.步骤三：利用预处理共轭梯度方法的专用矩阵迭代计算加速单元迭代求解矩阵方程，获得相机位姿参数的改变量；
20.步骤四：矩阵运算单元参与加速计算地图点空间改变量。
21.本发明还提供一种计算机存储介质，其上存储有计算机程序，该程序被处理器执行时实现第一方面所述方法的步骤。
22.与现有技术相比，本发明的有益效果为：
23.1.针对slam后端优化的一种加速架构，实现了基于光束平差法的后端优化硬件运算加速器，能够灵活适用于各种slam算法的后端优化组件的运算，具有配置灵活、运算速度快、功耗低等特点。
24.2.硬件加速器可以减少slam算法的计算代价，进而提升slam算法的性能。同时，结合软硬件层面设计的专用计算架构将提高计算能力，降低硬件成本，从而降低机器人设计和应用的门槛，提升slam系统性能以及可拓展空间。
25.3.更新了相机位姿参数以及地图点信息，通过硬件并行加速的计算框架实现了对slam后端优化算法的光束平差法的计算加速，以此提升slam系统硬件的计算算力，提高计算速度，节省计算时间，进而为slam算法扩展到更加复杂的算法奠定基础，实现slam系统的高性能，拓展slam系统的应用场景。
附图说明
26.图1为本发明提出的一种适用于slam非线性并行化芯片计算架构的整体结构示意图；
27.图2为本发明提出的一种以6维方阵矩阵乘加的流程图；
28.图3为本发明提出的一种舒尔补构造单元的结构示意图。
29.图4为本发明提出的预处理梯度下降矩阵求解器单元的结构示意图。
30.图5为本发明提出的slam后端优化硬件加速结构示意图。
具体实施方式
31.下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。
32.求解slam后端非线性优化问题的一个有效且常用的经典方法是光束平差法，在光束平差法中，相比于相机位姿参数的个数，地图点的数量能够达到成千上万，即使使用关键帧以及稀疏性的特征，每个关键帧通常能够观测到数百个关键点，且运算过程存在大量的矩阵操作，以及复杂的矩阵方程求解过程。具体的，slam算法后端优化运算过程中，将在舒尔补方程构造、大规模正定矩阵求解以及其他矩阵操作计算过程中花费大量时间。在当前主流的cpu上也将耗费大量的计算资源，对cpu的算力要求也较高。
33.本实施例实现光束平差法求解过程中，使用levenberg-marquart(lm)算法进行优化求解，如下式
[0034][0035]
其中x为待优化的参数，分为相机位姿参数(组)和地图点参数(m组)两部分。f(x)为透视投影函数，j(x)为参数x关于的雅可比矩阵，d(x)为雅可比矩阵的对角矩阵，∈为实际地图点与相机模型预测投影点之间的误差，λ为lm算法参数，δ
*
为计算所得优化改变量。令可以得到矩阵方程，如公式(2)：
[0036][0037]
由上述问题及其求解方式，研究slam后端线性优化具有以下特殊性质:
[0038]
第一，后端优化中矩阵运算是目前影响运算性能的主要瓶颈，在预计算雅可比矩阵、海森矩阵，通过舒尔消解方法如公式(3)可以实现大规模方程的消解，利用预处理共轭梯度法求解矩阵方程，这些关键步骤中均需要矩阵运算；
[0039]
第二，由于雅可比矩阵存在稀疏化的结构，使得公式(2)左式第一项存在特殊的稀疏化的块结构矩阵，且各块结构可以在各步骤计算中并行运算，互不影响，因此存在设计并行运算模块进行加速的方法和装置。
[0040]
[0041][0042]
本实施例实施slam算法后端优化部分硬件加速的计算架构如图1所示，主要分为矩阵运算加速单元，预处理共轭梯度法矩阵方程求解加速单元以及支持slam算法数据存储和算法控制的其他部分。利用通用处理器控制和调度在总线、存储模块、通用处理器以及专用的运算模块之间进行数据传输，利用矩阵运算加速单元实现快速的矩阵、向量、标量之间的乘法和加法运算，利用舒尔补矩阵并行化构造加速单元实现大规模矩阵方程的消解构造舒尔补矩阵方程，利用预处理共轭梯度法矩阵方程求解加速单元并行的求解矩阵方程的各个块结构。
[0043]
如图2所示，基于脉动阵列的方式进行运算，以一种数据流关系将运算单元形成一个运算网络，根据指令从两个待运算矩阵存储器起始地址读取所需数据，如图按时序定向输入到指定单元传入到多维运算单元中。每一个时钟周期，依次将多维运算单元的数据向右传递/向下传递，装置中的每个运算单元在运算过程中将输入数据进行乘法运算，加上该单元存储的中间值，得到该单元的运算结果，运算结果储存到本单元的同时，传递到相邻的右侧/下方运算单元。对于已经在运算单元中计算的数据，可以直接以中间值的形式在多维运算单元中传递，这里用了数据运算的重用性，从而大大减少了数据的搬运次数。重复上述计算和传递步骤，直到待运算矩阵全部输入到多维运算单元并计算完毕，将最终的结果输出至指令规定的存储目的地址保存，仅而完成整个运算指令流程。此外，本装置可以通过指令改变数据流定向输入的规模，从而实现1
×
3规模的向量、1
×
6规模的向量、3维方阵矩阵和3
×
6维矩阵之间的向量乘法、矩阵乘加等操作。
[0044]
如图3所示，本实施例提供的舒尔补构造单元结构，求解方程的等式左侧为3m+6n的方阵，可以通过舒尔补消解的方式将规模减小到3m规模的方阵，进而极大简化和加速之后的矩阵方程求解过程。
[0045]
该单元输入包括投影误差、相机参数雅可比矩阵以及地图点参数雅可比矩阵等六类矩阵，依据计算的数据依赖性分为了五个部分，共计12个计算阶段，同时几种不同的部分根据计算量的大小和数据的依赖性，拆分为若五个计算阶段以平衡计算延迟，增加计算速度。
[0046]
阴影部分为专用的矩阵乘加单元。
[0047]
第一个部分计算地图点的雅可比矩阵j
p
的转置与其本身的矩阵乘加v＝∑j
ptjp
，以及计算其矩阵的逆v-1
；
[0048]
第二个部分计算地图点的雅可比矩阵j
p
和投影误差e
p
的矩阵乘∈
p
，之后依次与矩阵v-1
、j
p
和jc进行矩阵乘加运算，得到jcj
p
v-1
∈
p
；
[0049]
第三个部分计算关于相机参数的雅可比矩阵jc和投影误差ec的矩阵乘∈c，与第一部分结果相减得到b
schur
＝∈
c-∑j
ctjp
v-1
∈
p
；
[0050]
第四个部分计算相机参数的雅可比矩阵jc的转置与地图点的雅可比矩阵jc的转置与的矩阵乘加运算w
t
＝∑j
pt
jc，计算其转置后，之后依次与矩阵v-1
、j
p
和jc进行矩阵乘加运算，得到j
ctjp
v-1wt
；
[0051]
第五个部分计算相机参数的雅可比矩阵jc的转置与其本身的矩阵乘加u＝∑j
ct
jc，之后计算与第四部分计算结果的矩阵减法，h
schur
＝u-∑j
ctjp
v-1wt
。
[0052]
五个计算阶段的产生的中间变量存储在片上ram中或寄存器堆中，同时扩展计算阶段之间的ram大小形成ping-pong缓冲的结构以提升计算的并行度。自此该单元完成了舒尔补消解后的求解方程的构造。
[0053]
如图4所示，本实施例加速单元包括了矩阵乘法(虚线框表示)、向量点乘(双实线框表示)、向量axpy运算(阴影框表示)以及标量运算(单实线框表示)四种运算。
[0054]
具体说明：
[0055]
预处理梯度下降矩阵求解方法根据计算量的大小和数据的依赖性，拆分为若三个计算阶段以平衡计算延迟，增加计算速度。在使用该单元求解前，首先根据算法，对各部分数据如算法1的第一行进行初始化，其中ax＝b等式对应本实施例中矩阵有如下关系，a：＝h
schur
，b：＝b
schur
。
[0056]
第一个阶段计算算法1中的行8、行9、行10；
[0057]
第二个步骤计算算法行5和行4步骤，计算分为两个部分并行进行；
[0058]
第三阶段计算算法中行6、行7，计算同样可以分为两个部分并行进行。
[0059]
每次迭代产生的矩阵x，r，w，p将作为待更新数据参与下一轮迭代，直至达到算法1中的行2、行3所满足的退出条件，
[0060]
最终得到优化后的矩阵x。三个计算阶段的产生的中间变量存储在片上ram中或寄存器堆中，同时扩展计算阶段之间的ram大小形成ping-pong缓冲的结构以提升计算的并行度。自此该单元完成了预处理梯度下降矩阵求解，得到优化后的矩阵x，即相机参数的优化矩阵。
[0061]
算法1预处理共轭梯度算法
[0062][0063]
如图5所示，本实施例是要求能够并行化地加速slam算法后端优化的运算过程，减少数据交换，节约存储空间。
[0064]
为了更高效率的控制slam后端优化流程，通过总线连接且主要加速预处理矩阵运算部分、舒尔补构造矩阵方程以及pcg求解正定矩阵方程的运算，故各个单元采用流水线的方式执行，也可以并发地执行。
[0065]
以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，根据本发明的技术方案及其
发明构思加以等同替换或改变，都应涵盖在本发明的保护范围之内。