一种用于SLAM的已知自身位姿的光束平差法FPGA加速器专利检索-软件固件电脑零配件专利检索查询-专利查询网

一种用于SLAM的已知自身位姿的光束平差法FPGA加速器

阅读：1028发布：2020-06-25

专利汇可以提供一种用于SLAM的已知自身位姿的光束平差法FPGA加速器专利检索，专利查询，专利分析的服务。并且本发明公开一种用于SLAM的已知自身位姿的光束平差法FPGA 加速器，其雅各比矩阵更新和成本函数计算单元通过Zynq的PL端电路实现，PL端电路包括与PL端的控制器连接的相机旋转处理单元、三维点投影处理单元，所述相机旋转处理单元、三维点投影处理单元的输入端各自通过一个输入FIFO连接片外存储器，所述相机旋转处理单元的输出端与片上存储器的输入端连接，所述片上存储器的输出端连接所述三维点投影处理单元的输出端，所述三维点投影处理单元的结果输出端通过输出FIFO与片外存储器的计算结果输入端连接。本发明可通过时分复用的方式利用单一硬件同时完成雅各比矩阵更新和成本函数计算。，下面是一种用于SLAM的已知自身位姿的光束平差法FPGA加速器专利的具体信息内容。

权利要求

1.一种用于SLAM的已知自身位姿的光束平差法FPGA加速器，其特征在于，采用光束平差法实现，其中的雅各比矩阵更新和成本函数计算单元通过Zynq的PL端电路实现，PS部分实现光束平差法的其余计算，PL端电路包括与PL端的控制器连接的相机旋转处理单元、三维点投影处理单元，所述相机旋转处理单元、三维点投影处理单元的输入端各自通过一个输入FIFO连接片外存储器，所述相机旋转处理单元的输出端与片上存储器的输入端连接，所述片上存储器的输出端连接所述三维点投影处理单元的输出端，所述三维点投影处理单元的结果输出端通过输出FIFO与片外存储器的计算结果输入端连接；
所述相机旋转处理单元、三维点投影处理单元通过时分复用的方式同时完成雅各比矩阵更新和成本函数的计算；相机旋转处理单元，用于完成相机旋转的计算，求出相机旋转矩阵；三维点投影处理单元，用于完成投影值，投影值相对输入的导数以及成本函数值的计算。
2.根据权利要求1所述用于SLAM的已知自身位姿的光束平差法FPGA加速器，其特征在于，所述相机旋转处理单元、三维点投影处理单元内部拆分成不同的计算单元，不同的计算单元之间采用FPGA片上双端口RAM进行数据传递，不同的计算单元内部采用有限状态机，通过状态转换来控制每步数据的读入、读出和存储。
3.根据权利要求1所述用于SLAM的已知自身位姿的光束平差法FPGA加速器，其特征在于，加速器使用猜测执行改进软件算法，使用猜测执行，在LM算法计算pnew处成本函数值的同时，计算了在pnew处的雅各比矩阵J，对角矩阵D以及残差∈，根据LM算法的增益比ρ来确定LM下一次迭代输入的数据时新的J，D，∈，还是旧的J，D，∈。
4.根据权利要求1所述用于SLAM的已知自身位姿的光束平差法FPGA加速器，其特征在于，加速器通过复制硬件中的三维点投影处理单元提升计算的并行度，以提高计算速度。
5.根据权利要求1所述用于SLAM的已知自身位姿的光束平差法FPGA加速器，其特征在于，所述的相机旋转处理单元、三维点投影处理单元内部采用流水线技术模块复用技术。
6.根据权利要求1所述用于SLAM的已知自身位姿的光束平差法FPGA加速器，其特征在于，相机旋转处理单元根据输入相机参数计算相机旋转矩阵，将旋转矩阵和其他相机参数暂存到片上RAM中，之后三维点投影处理单元从片上RAM读取相机数据和输入缓存中读取三维点位置计算最终的投影位置，并完成雅各比矩阵更新和成本函数计算的任务。
7.根据权利要求1所述用于SLAM的已知自身位姿的光束平差法FPGA加速器，其特征在于，相机旋转处理单元计算相机旋转矩阵，对应投影函数中的罗德里格斯旋转公式，采用串行执行以节约计算资源，根据计算的数据的依赖性分为两个部分三个计算阶段，每两个计算阶段间通过片上RAM交互数据；第一部分两个阶段通过输入的旋转矢量r计算旋转角θ，通过坐标旋转数字计算方法计算θ的三角函数值sinθ和cosθ；第二部分一个阶段进行罗德里T
格斯旋转公式中剩余乘加操作，且将该罗德里格斯旋转公式中最后一项的向量乘r r提前到第一阶段进行计算，三个计算阶段的产生的中间变量存储在片上RAM中或寄存器堆中，同时扩展计算阶段之间的RAM大小形成Ping-pong缓冲的结构以提升计算的并行度，第三阶段将所有相机的旋转矩阵计算结果R和其余相机参数存储到片上RAM中，供后续三维点投影处理单元读取计算，罗德里格斯旋转公式如下：
其中
三维点投影处理单元根据输入相机参数和三维点的世界坐标计算点在相机平面的投影位置与实际投影位置的误差和投影位置对点坐标的偏导数，并同时计算对角矩阵D和成本函数值三维点投影处理单元根据计算的数据依赖性分为了五个部分共八个计算阶段，其中不同的部分根据计算量的大小和数据的依懒性，拆分为1-3个计算阶段以平衡计算延迟，增加计算速度，每个两个阶段之间的数据交互也通过片上RAM；
其中，第一部分一个计算阶段计算三维点在相机坐标系下的位置Xc,Yc,Zc；第二部分两个阶段计算点投影在归一化坐标平面的位置x,y；第三部分三个计算阶段计算投影径向失真的大小d；第四部分一个计算阶段先计算含有失真的投影的位置u,v，再根据输入的观测值的真实值计算重投影与实际投影的残差∈和雅各比矩阵J，最后一部分一个计算阶段使用乘累加计算LM算法的对角矩阵D和成本函数值

说明书全文

一种用于SLAM的已知自身位姿的光束平差法FPGA加速器

技术领域

[0001] 本发明涉及FPGA器件技术领域，特别是涉及用于SLAM的已知自身位姿的光束平差法FPGA加速器。

背景技术

[0002] 光束平差法(Bundle Adjustment,BA)是在三维重建中联合优化相机参数和三维结构特征点位置的问题，广泛应用于三维立体场景的重建和同步定位和地图构建(SLAM)。BA的目标是通过最小化重投影误差来找到最优的参数估计，即三维空间点位置和相机参数。该误差被定义为观察到的特征位置与根据输入参数计算点在相机图像平面上位置差的L2范数。

[0003] SLAM赋予移动机器人在没有预知地图信息的条件下定位自身和对周围环境建图的能力。BA在SLAM中主要应用于SLAM后端，对前端已经建立的具有累积误差的地图进行优化。在SLAM发展的初期，后端使用扩展卡尔兹曼滤波算法。之后，基于BA的SLAM系统逐渐被开发出来，而且已经证明BA的图优化方法在性能和精度上相较滤波算法更适合作为SLAM后端。

[0004] 但目前SLAM算法距实际应用仍有一定的距离，其中BA是限制其应用的性能和功耗瓶颈。目前的研究大部分针对BA性能的研究使用GPU或分布式处理，由于其功耗非常大，无法很好的应用到SLAM的嵌入式场景。

发明内容

[0005] 本发明的目的是针对现有技术中存在的技术缺陷，而提供一种用于SLAM的已知自身位姿的光束平差法FPGA加速器。

[0006] 为实现本发明的目的所采用的技术方案是：

[0007] 一种用于SLAM的已知自身位姿的光束平差法FPGA加速器，采用光束平差法实现，其中的雅各比矩阵更新和成本函数计算单元通过Zynq的PL端电路实现，PS部分实现光束平差法的其余计算，PL端电路包括与PL端的控制器连接的相机旋转处理单元、三维点投影处理单元，所述相机旋转处理单元、三维点投影处理单元的输入端各自通过一个输入FIFO连接片外存储器，所述相机旋转处理单元的输出端与片上存储器的输入端连接，所述片上存储器的输出端连接所述三维点投影处理单元的输出端，所述三维点投影处理单元的结果输出端通过输出FIFO与片外存储器的计算结果输入端连接；

[0008] 所述相机旋转处理单元、三维点投影处理单元通过时分复用的方式同时完成雅各比矩阵更新和成本函数的计算；相机旋转处理单元，用于完成相机旋转的计算，求出相机旋转矩阵；三维点投影处理单元，用于完成投影值，投影值相对输入的导数以及成本函数值的计算。

[0009] 本发明通过FPGA的并行计算来提嵌入式中光束平差法的计算速度，满足嵌入式SLAM中已知相机位姿BA优化建图的实时性要求。

[0010] 本发明可以通过时分复用的方式利用单一硬件同时完成雅各比矩阵更新和成本函数计算，可以通过复制多个RPP以提升硬件加速器的并行度，改进软件算法使用猜测执行提升计算效率。附图说明

[0011] 图1是本发明基于Zynq FPGA的已知自身位姿的光束平差法加速器的硬件架构框图；

[0012] 图2是本发明旋转矩阵处理器(RMP)的硬件结构图；

[0013] 图3是本发明重投影处理器(RPP)的硬件结构图。

具体实施方式

[0014] 以下结合附图和具体实施例对本发明作进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

[0015] 当SLAM应用设备已知自身的位姿，SLAM后端仅需要完成建图任务，求解BA的过程中无需优化相机参数，在这种情况下，求解BA问题过程中，雅各比矩阵的更新和成本函数的计算是计算量最大，是系统中最耗时的部分。

[0016] 在本发明的测试中，两者之和可以达到总计算时长的70％以上。优化方法使用经典求解BA的Levenberg-Marquardt(LM)算法。

[0017] 如图1所示，基于Zynq FPGA的已知自身位姿的光束平差法加速器如图1所示，Zynq芯片分为PS与PL两部分，其中PL部分上实现了雅各比矩阵的更新和成本函数的计算，PS部分实现了其余计算。

[0018] 软件算法中为了计算的一致性，需要同时输入相机参数和三维点位置才能计算投影位置，计算每个点的每一次投影位置都需要计算相机的旋转，这样就造成了不同点在同一相机投影下对相机旋转矩阵的重复计算，而硬件设计就避免了这一缺陷。

[0019] 所述硬件的PL部分根据投影算法的特性拆分为两个部分，分别为RMP和RPP。RMP根据输入相机参数计算相机旋转矩阵，将旋转矩阵和其他相机参数暂存到片上RAM中，之后RPP从片上RAM读取相机数据和输入缓存中读取三维点位置计算最终的投影位置，并完成雅各比矩阵更新和成本函数计算的任务。

[0020] RMP计算相机旋转矩阵，对应投影函数中的罗德里格斯旋转公式(1)。由于旋转矩阵计算的时间较短，RMP模块近似采用串行执行以节约计算资源，但根据计算的数据的依赖性划分为了两个部分三个计算阶段，每两个计算阶段之间通过片上RAM来交互数据。

[0021] 其中，第一部分两个阶段主要通过输入的旋转矢量r计算旋转角θ；其中为保证计算精度通过坐标旋转数字计算方法(CORDIC)计算θ的三角函数值sinθ和cosθ；第二部分一个阶段进行该公式中剩余的乘加操作，为了平衡三个计算阶段的延时，将该公式中最后一项的向量乘rTr提前到第一阶段进行计算。三个计算阶段的产生的中间变量存储在片上RAM中或寄存器堆中，同时扩展计算阶段之间的RAM大小形成Ping-pong缓冲的结构以提升计算的并行度。第三阶段将所有相机的旋转矩阵计算结果R和其余的相机参数存储到片上RAM中，供后续RPP模块读取计算。

[0022] 其中 (1)

[0023] RPP根据输入相机参数和三维点的世界坐标计算点在相机平面的投影位置与实际投影位置的误差和投影位置对点坐标的偏导数，并同时计算对角矩阵D和成本函数值与RMP相似，RPP也根据计算的数据依赖性分为了五个部分共八个计算阶段，其中不同的部分根据计算量的大小和数据的依懒性，拆分为1～3个计算阶段以平衡计算延迟，增加计算速度，每个两个阶段之间的数据交互也通过片上RAM。

[0024] 其中，第一部分一个计算阶段计算三维点在相机坐标系下的位置Xc,Yc,Zc；第二部分两个阶段计算点投影在归一化坐标平面的位置x,y；第三部分三个计算阶段计算投影径向失真的大小d；第四部分一个计算阶段先计算含有失真的投影的位置u,v，再根据输入的观测值的真实值计算重投影与实际投影的残差∈和雅各比矩阵J，最后一部分一个计算阶段使用乘累加计算LM算法的对角矩阵D和成本函数值

[0025] 由于硬件设计的时分复用思想，在RPP中，每一阶段除了计算函数值之外，还计算了它们相对于点三维空间位置的偏导数，以更新雅各比矩阵。

[0026] 雅各比矩阵更新和成本函数部分在计算中会使用相同的中间变量，为避免重复计算、节约硬件资源，降低系统功耗，通过时分复用计算资源，将雅各比矩阵更新与成本函数计算在同一硬件实现，使硬件在更新雅各比矩阵的同时计算了BA的成本函数值。

[0027] 同时改进软件算法，使用猜测执行，在LM算法计算pnew处成本函数值的同时，计算了在pnew处的雅各比矩阵J，对角矩阵D以及残差∈，根据LM算法的增益比ρ来确定LM下一次迭代输入的数据时新的J，D，∈，还是旧的J，D，∈。

[0028] 在BA问题中，由于相机数量远小于投影的数量，重投影计算的时间占据了绝大部分的计算时间，所述的加速器设计中可以复制多个RPP模块以提升系统处理的并行度，减少系统的计算时间。

[0029] 将上述硬件设计在Zedboard开发板上综合实现，就资源利用率、运行时间和功率消耗这三方面，对该硬件设计进行性能测试。

[0030] 光束平差法是指计算的相机参数和三维空间点的位置，即移动机器人的轨迹和地图结构。BA旨在将三维空间点在图片上的真实投影位置与它根据输入参数计算出的预测投影位置之间的差异最小化来达到优化参数的目的。假设pi为第i个三维点的位置，cj为第j张图片的相机参数，oij为第i个三维点在第j张图片上的真实的位置，P(pi，cj)为投影函数，则光束平差法的成本函数可以被描述为：

[0031]

[0032] σij表示表示第i个三维点是否被第j张图片观测到，由于空间位置遮挡的关系，一张图片并不是观测到所有的三维点，当第i个三维点被第j张图片观测到时σij＝1，否则σij＝0。当SLAM应用设备已知自身的位姿，BA作为SLAM后端仅需要完成建图任务。

[0033] Levenberg-Marquardt(LM)算法是一种非线性最小二乘法，被广泛用于寻找非线性函数的局部最小值。其是一种基于信任域的算法，因为每次寻找的步长不一定能较小成本函数，在每次迭代计算新候选点pnew后，需要根据新的成本值和旧的成本值判定是否接受新的候选点pnew，同时计算新一次迭代的信任域半径。

[0034] 下面对该硬件架构进行测试，下文对测试方法进行介绍：

[0035] 根据设计的硬件结构，在Xilinx Vivado 2017.04上编写RTL代码并综合实现，再下载到Zedboard开发板上进行板级调试。RMP所能达到的时最大钟频率为50MHz，RPP的最大的时钟频率为150MHz。就硬件设计的资源利用率、加速比和功耗三方面，本发明进行了性能评估。评估所使用的的数据集为BundleAdjustment in the Large。

[0036] 资源利用率：由Xilinx Vivado综合实现后得到，数据精度使用单精度浮点数。

[0037] 运行时间：软件实现选用来自于Google的开源的Ceres-Solver最优化库，使用双精度浮点数编译。X86平台选用Inteli5-8400，主频为2.8GHz，ARM平台选用Zynq-7000芯片中的ARMCortex-A9处理器，主频为667MHz。

[0038] 功率消耗：通过XilinxPowerEstimator计算的功率值。

[0039] 所述的加速器测试结果如下：

[0040] 硬件加速器消耗的硬件资源：查找表42936，占81％；触发器59230，占56％，BRAM80.5，占58％，DSP162，占74％。

[0041] Intel x86，ARM和FPGA平台在测试使用的五个数据集上BA的平均执行时间分别为51.029ms，1363.232ms和228.646ms，FPGA平台的性能是ARM平台的5.962倍，可以达到嵌入式实时建图的要求。在三个平台能量平均消耗分别为3316.9mJ，2044.8mJ，640.2mJ，FPGA平台相比Intel，ARM平台节约80.7％，68.7％的能量。

[0042] 以上所述仅是本发明的优选实施方式，应当指出的是，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

标题	发布/更新时间	阅读量
用于流处理的数据处理单元	2021-02-14	0
基于MDL文件的数据提取方法、装置、设备及存储介质	2022-02-25	1
云终端升级处理方法及装置	2020-12-30	2
一种复杂结构铸件整体砂型的灌涂方法	2021-09-23	2
一种根据应用包特征自动生成容器镜像的智能打包方法	2023-02-12	1
使用照明装置自动指示联系中心状态的系统和方法	2020-05-23	2
光学成像系统及设计方法	2020-09-25	2
用于长期运行的软件操作的连接器租用	2020-10-12	2
一种使用卡封片锁定的无钥匙锁具	2022-09-05	1
单手游戏键盘	2021-10-07	0

一种用于SLAM的已知自身位姿的光束平差法FPGA加速器

一种用于SLAM的已知自身位姿的光束平差法FPGA加速器

技术领域

背景技术

发明内容

具体实施方式

该功能需要专业版企业版VIP权限，您可以：