CN115168283A - 一种数据融合模块加速器及方法 - Google Patents

一种数据融合模块加速器及方法 Download PDF

Info

Publication number
CN115168283A
CN115168283A CN202210784735.2A CN202210784735A CN115168283A CN 115168283 A CN115168283 A CN 115168283A CN 202210784735 A CN202210784735 A CN 202210784735A CN 115168283 A CN115168283 A CN 115168283A
Authority
CN
China
Prior art keywords
rotation
matrix
calculation
visual
module
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202210784735.2A
Other languages
English (en)
Inventor
王锐
刘轶
吕向
黄一迪
钱德沛
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beihang University
Original Assignee
Beihang University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beihang University filed Critical Beihang University
Priority to CN202210784735.2A priority Critical patent/CN115168283A/zh
Publication of CN115168283A publication Critical patent/CN115168283A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F15/00—Digital computers in general; Data processing equipment in general
    • G06F15/76—Architectures of general purpose stored program computers
    • G06F15/78—Architectures of general purpose stored program computers comprising a single central processing unit
    • G06F15/7807—System on chip, i.e. computer system on a single chip; System in package, i.e. computer system on one or more chips in a single package
    • G06F15/7817—Specially adapted for signal processing, e.g. Harvard architectures
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Hardware Design (AREA)
  • Mathematical Physics (AREA)
  • General Engineering & Computer Science (AREA)
  • Data Mining & Analysis (AREA)
  • Pure & Applied Mathematics (AREA)
  • Computational Mathematics (AREA)
  • Mathematical Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Computing Systems (AREA)
  • Algebra (AREA)
  • Microelectronics & Electronic Packaging (AREA)
  • Signal Processing (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Complex Calculations (AREA)

Abstract

本发明公开了一种数据融合模块加速器及方法，包括：旋转四元数转旋转矩阵加速模块、视觉重投影误差数据处理加速模块、IMU预积分数据处理加速模块、累加模块、片外存储器。有效平衡加速和精度要求，并降低功耗开销，有更好的能效比，为VIO后端优化算法在算力和功耗有限的嵌入式平台部署实现完整部署提供重要设计思路和设备模块。基于本发明的视觉惯性里程计原型机能够实现超过30FPS的实时运行速率，与Intel i5‑9400F处理器平台相似，并且远高于Arm cortex‑A53嵌入式平台的运行效果。

Description

一种数据融合模块加速器及方法
技术领域
本发明涉及非线性优化硬件加速器、视觉SLAM和FPGA的硬件设计与装置，特别是一种数据融合模块加速器及方法。
背景技术
视觉里程计(Visual Odometry,VO)是通过分析一系列关键帧序列确定机器人的朝向和位置的过程。而同步定位与地图构建(SLAM)主要用于解决移动机器人在未知环境中运动时定位导航与地图构建的问题，视觉里程计可以看作是其的一个模块。随着计算机与无人系统的飞速发展，为了方便视觉SLAM系统在小型无人嵌入式平台上的部署，许多研究使用硬件对视觉惯性里程计进行了加速。2017年，Fang等人在FPGA上设计了ORB-SLAM硬件加速模块，针对特征点视觉SLAM算法中的特征提取部分实现了硬件加速，在相比桌面级CPU功耗降低83％的情况下，达到了近一倍的速度提升。该方法发表在论文[Fang W,Zhang Y,Yu B,et al.FPGA-based ORB feature extraction for real-time visual SLAM[C]//2017 International Conference on Field Programmable Technology(ICFPT).IEEE,2018].
视觉里程计的后端优化主要有两种方案，早期研究由于算力的限制主要采用以扩展卡尔曼滤波(Extended Kalman Filter，EKF)为主的滤波器方案，近期研究开始采用非线性优化方案进行更准确的精度优化。由Maoteng Zheng等人提出了一种快速精确的束调整方法，该方法仍然使用传统的Levenberg Marquardt(LM)算法框架，同时采用预处理共轭梯度(PCG)迭代求解正规方程，并使用点重采样方案和引入了一种先进的BSMC格式来存储稀疏矩阵最大化正常矩阵的块特性来降低内存需求和计算复杂度。该方法发表在论文[ZhengM,Zhang F,Zhu J,et al.A fast and accurate bundle adjustment method for verylarge-scale data[J].Computers&Geosciences,2020,142:104539].
Jeong等人给出了几种在计算和收敛性方面表现良好的集束调整方法(BundleAdjustment，BA)以及它们的实验结果。并利用高效的内存处理和快速的基于块的线性求解，提出了一种新的嵌入点迭代方法，大大提高了BA在CPU上的性能。该方法发表在论文[Jeong Y,Nister D,Steedly D,et al.Pushing the envelope of modern methods forbundle adjustment[J].IEEE transactions on pattern analysis and machineintelligence,2011,34(8):1605-1617].
Wu等提出了一种运行在CPU和GPU上的多核并行处理解决方案。在这种设计中，矩阵向量乘积被仔细地重构，以显著降低内存需求和计算延迟。该方法发表在论文[Wu C,Agarwal S,Curless B,et al.Multicore bundle adjustment[C]//CVPR 2011.IEEE,2011:3057-3064].
Liu等提出了一种软硬件协同设计的BA硬件加速器及其在嵌入式FPGA-SoC上的实现，以同时实现更高的性能和功率效率。特别地，提出了一种协同观测优化技术和一种硬件友好的差分方法，以优化内存和计算资源的使用，加速BA操作。该方法发表在论文[Liu Q,Qin S,Yu B,et al.$\pi$π-BA:Bundle Adjustment Hardware Accelerator Based onDistribution of 3D-Point Observations[J].IEEE Transactions on Computers,2020,69(7):1083-1095]。
然而现有技术对VIO后端优化求解器的硬件加速效果不能保证算法精度和计算速度之间的平衡，针对这个情况和VIO后端优化算法本身难以在算力和功耗有限的嵌入式平台部署的问题，本发明实现了一种数据融合模块加速器及方法。该方法可以在不损失精度的前提下,提升VIO后端优化在硬件平台的运算速率和能效比。在此基础上实现的基于FPGA平台的数据融合模块加速器的VIO后端优化原型机运行速率比嵌入式平台Arm cortex-A53快近4倍(接近桌面级处理器水平)，同时比嵌入式平台Arm cortex-A53降低了50％的能耗,有更好的能效比。
发明内容
本发明技术解决问题：针对现有VIO后端优化求解器通常以降低精度要求为代价实现加速，且VIO后端优化算法本身难以在算力和功耗有限的嵌入式平台部署的问题，提供一种数据融合模块加速器及方法，有效平衡加速和精度要求，并降低功耗开销，有更好的能效比，为VIO后端优化算法在算力和功耗有限的嵌入式平台部署实现完整部署提供重要设计思路和设备模块。基于本发明的视觉惯性里程计原型机能够实现超过30FPS的实时运行速率，与Intel i5-9400F处理器平台相似，并且远高于Arm cortex-A53嵌入式平台的运行效果。
本发明技术解决方案之一：
一种数据融合模块加速器，包括：旋转四元数转旋转矩阵加速模块、视觉重投影误差数据处理加速模块、IMU预积分数据处理加速模块、累加模块、片外存储器；
片外存储器，用于存储通过视觉惯性SLAM视觉惯性里程计VIO前端得到的旋转四元数、视觉观测信息、先验信息和IMU预积分信息；所述视觉观测信息包括不同相机观测的空间点集、相机采集的关键帧集、关键帧特征点集和特征点匹配点集；所述IMU预积分信息包括位置信息、方向信息和速度信息；
旋转四元数转旋转矩阵加速模块，包括旋转四元数转旋转矩阵加速策略；片外存储器中的旋转四元数传入旋转四元数转旋转矩阵加速模块，利用旋转四元数转旋转矩阵加速策略加速旋转矩阵和旋转逆矩阵的计算；所得旋转矩阵和旋转逆矩阵被传入视觉重投影误差加速模块和IMU预积分数据处理加速模块；
视觉重投影误差加速模块包含视觉重投影误差加速策略；片外存储器中的视觉观测信息、旋转四元数转旋转矩阵加速模块输出的旋转矩阵和旋转逆矩阵传入视觉重投影误差加速模块，依次进行视觉重投影海森矩阵计算、舒尔补消除计算，计算过程中利用视觉重投影误差加速策略提升舒尔补消除计算速度；所得视觉重投影海森矩阵计算分量S1和视觉重投影残差计算分量r1被传入累加模块；
IMU预积分数据处理加速模块包含数据处理加速策略；片外存储器中的先验信息、IMU预积分信息和和旋转四元数转旋转矩阵加速模块输出的旋转矩阵和旋转逆矩阵传入IMU预积分数据处理加速模块进行滑动窗口先验海森矩阵计算、IMU预积分海森矩阵计算，计算过程中利用数据处理加速策略提升IMU预积分海森矩阵计算中涉及的旋转矩阵转旋转四元数的计算速度，并限制滑动窗口先验海森矩阵计算的规模；所得滑动窗口先验海森矩阵和IMU预积分海森矩阵的整合计算分量S2、滑动窗口先验残差和IMU预积分残差的整合计算分量r2被传入累加模块；
累加模块基于传入的视觉重投影海森矩阵计算分量S1、视觉重投影残差计算分量r1、IMU预积分海森矩阵的整合计算分量S2、IMU预积分残差的整合计算分量r2执行累加操作，得到的VIO后端优化海森矩阵计算分量S和VIO后端优化残差计算分量r传入片外存储器；片外存储器中的S和r用于后续的VIO后端非线性优化的计算。
进一步，所述旋转四元数转旋转矩阵加速模块中，旋转四元数转旋转矩阵加速策略包括四个部分：第一，使用4个double 64类型的数据表示旋转四元数,共32个字节；第二，旋转四元数转换为旋转矩阵的过程为：根据输入的旋转四元数直接计算其对应的旋转矩阵；第三，旋转四元数转换为旋转逆矩阵的过程为：先将输入的旋转四元数执行虚部取反，然后计算取反后的旋转矩阵；第四，将第二与第三在四元数转旋转矩阵模块中并行处理以提高计算速度。
进一步，所述视觉重投影误差加速模块中，视觉重投影误差数据处理加速策略；所述视觉重投影误差数据处理加速策略用于简化舒尔补消除计算的复杂度，以加速S1和r1的计算，方法为取视觉重投影海森矩阵上对应位置存在共视关系的特征点数据，执行舒尔补消除计算；所述公视关系为不同相机在同一时间观测到的相同空间点。
进一步，所述IMU预积分数据处理加速模块中，数据处理加速策略包括滑动窗口长度设置、旋转矩阵转旋转四元数加速策略；
所述滑动窗口长度设置，用于限制滑动窗口先验海森矩阵的计算规模，滑动窗口长度设置为11；
所述旋转矩阵转旋转四元数加速策略，用于加速旋转矩阵转旋转四元数计算，包括：第一，使用长度为9个浮点数类型的数据表示旋转矩阵；第二，通过求解旋转矩阵的迹和使用快速平方根倒数方法，以少量精度损失为代价提高求解旋转矩阵对应旋转四元数的计算速度。
进一步，所述数据融合模块加速器基于FPGA平台实现，旋转四元数转旋转矩阵加速模块、视觉重投影误差数据处理加速模块、IMU预积分数据处理加速模块、累加模块均部署于FPGA平台的PL端，使用AXI4总线协议，与片外存储器通过4条AXI数据总线连接以提升数据传输的吞吐量。
本发明技术解决方案之二：一种数据融合模块加速方法，实现步骤如下：
步骤1：通过视觉惯性SLAM视觉惯性里程计VIO前端所得旋转四元数、视觉观测信息、先验信息、IMU预积分信息存入片外存储器；
步骤2：基于步骤1中的旋转四元数，旋转四元数转旋转矩阵加速模块采用旋转四元数转旋转矩阵加速策略实现计算加速，得到旋转矩阵和旋转逆矩阵；
步骤3：基于步骤1中的视觉观测信息、步骤2得到的旋转矩阵和旋转逆矩阵，视觉重投影误差加速模块采用视觉重投影误差加速策略，实现对视觉重投影海森矩阵的舒尔补消除计算的加速，得到视觉重投影雅可比矩阵、视觉重投影海森矩阵、视觉重投影海森矩阵计算分量S1和视觉重投影残差计算分量r1；
步骤4：基于步骤1中的先验信息、IMU预积分信息、步骤2得到的旋转矩阵和旋转逆矩阵，IMU预积分数据处理加速模块采用数据处理加速策略，实现滑动窗口先验海森矩阵和IMU预积分海森矩阵计算的加速，得到滑动窗口先验海森矩阵、IMU预积分海森矩阵、IMU预积分雅可比矩阵、IMU预积分和滑动窗口先验相关的海森矩阵计算分量S2、IMU预积分和滑动窗口先验相关的残差计算分量r2；
步骤5：基于步骤3和步骤4输出的S1、r1、S2、r2执行累加操作，得到累加后海森矩阵计算分量S和累加后残差计算分量r存入片外存储器；
步骤6：步骤5中所得S和r用于后续的VIO后端非线性优化的计算。
本发明与现有技术相比的有益效果是：
(1)本发明提供一种可以完整部署于FPGA平台的数据融合模块加速器及方法，可有效平衡加速和精度要求，并降低功耗开销。为VIO后端优化求解器在算力有限平台的部署以及视觉惯性SLAM系统在小型无人系统和智能嵌入式系统的发展提供了技术支持。
(2)数据融合模块加速器及方法中设计的旋转四元数转旋转矩阵加速模块，可有效规避传统旋转逆矩阵计算方法中涉及的乘除操作，并实现对旋转矩阵和旋转逆矩阵的并行计算。
(3)数据融合模块加速器及方法根据视觉重投影海森矩阵、IMU预积分海森矩阵和滑动窗口先验海森矩阵的特点，将需要参与舒尔补消除计算的视觉重投影海森矩阵分离，将求解过程类似的IMU预积分海森矩阵和滑动窗口先验海森矩阵的计算过程合并，从而将用于VIO后端优化计算的S和r拆分整合为S1、r1和S2、r2两部分。这两部分分别在视觉重投影误差加速模块和IMU预积分数据处理加速模块中完成计算，有效避免传统方法中舒尔补消除计算过程种的冗余运算。由于视觉重投影误差加速模块和IMU预积分数据处理加速模块在硬件中被单独设计，且模块之间没有数据依赖，因此可以实现并行运行处理，近一步实现了加速。
(4)在本发明的基础上，实现的视觉惯性里程计原型机能够实现超过30FPS的实时运行速率，与Intel i5-9400F处理器平台相似，并且远高于Arm cortex-A53嵌入式平台的运行效果。此外，本发明可有效降低功耗开销，有更好的能效比。
附图说明
图1是本发明数据融合模块加速器框架图；
图2是本发明视觉重投影海森矩阵和视觉重投影残差矩阵示意图；
图3是本发明存在共视关系的空间点的示意图；
图4是本发明视觉重投影误差加速策略的计算流程；
图5是本发明旋转四元数转旋转矩阵计算流程图；
图6基于数据融合模块加速器的视觉惯性里程计原型机工作流程图。
具体实施方式
下面结合附图及实施例对本发明进行详细说明。
1.数据融合模块加速器设计思路与整体框架
数据融合模块加速器用于加速VIO后端非线性优化中海森矩阵计算分量S和残差计算分量r的计算，其主要设计思路如下：
首先，VIO后端非线性优化方程F(χ)如下所示：
Figure BDA0003731493400000051
其中的eprior、eIMU和eimage分别为滑动窗口先验误差、IMU预积分误差和视觉重投影误差，χ为VIO后端非线性优化方程的状态量。根据LM算法整理上述方程，得到VIO后端非线性优化方程对应的线性方程：
(Hprior+HIMU+Himage+λI)△χ＝(bprior+bIMU+bimage)
上式中，Hprior、HIMU和Himage分别为滑动窗口先验误差、IMU预积分误差和视觉重投影误差所对应的海森矩阵模块，bprior、bIMU和bimage分别为滑动窗口先验误差、IMU预积分误差和视觉重投影误差所对应的残差矩阵，λ为阻尼因子参数，Δχ为VIO后端非线性优化方程的状态变化量，Δxc和Δxm分别为与相机和空间点相关的状态变化量。
根据Hprior、HIMU和Himage的性质整理出关于雅可比矩阵的分块矩阵，将上述VIO后端非线性优化方程对应的线性方程改写为如下形式，其中带有prior下标的矩阵数据由Hprior和bprior整理得到，带有IMU下标的矩阵数据由HIMU和bIMU整理得到，带有image下标的矩阵数据由Himage和bimage整理得到：
Figure BDA0003731493400000061
上式中，C表示在海森矩阵中与相机相关的数据分块，M表示在海森矩阵中与空间点相关的数据分块，E表示在海森矩阵中与相机和空间点都相关的数据分块，bm和bc分别表示与空间点和相机相关的残差矩阵分块数据，海森矩阵包括Hprior、HIMU和Himage，残差矩阵包括bprior、bIMU和bimage。将上式进行舒尔补运算，得到如下线性方程：
Figure BDA0003731493400000062
整理得：
Figure BDA0003731493400000063
上式中，Cprior表示Hprior中的C矩阵，CIMU表示HIMU中的C矩阵，
Figure BDA0003731493400000064
表示bprior中的bc，
Figure BDA0003731493400000065
表示bIMU中的bc，令S1＝(C-EM-1ET)image，S2＝Cprior+CIMU，r1＝(bc-EM-1bm)image，
Figure BDA0003731493400000066
S1、r1中所含C、M、E、bm、bc均为Himage和bimage中的数据分块，可得如下简化后线性方程：
(S1+S2+λI)Δxc＝(r1+r2)
(S+λI)Δxc＝r
此时，有S＝S1+S2，r＝r1+r2，即VIO后端非线性优化被分为S1、r1和S2、r2两个部分：S2和r2表示与IMU预积分和滑动窗口先验相关的计算量，S1和r1表示与视觉重投影误差部分相关的计算量。前者不需要进行舒尔补计算，同时两者之间并没有数据依赖关系，为了避免计算舒尔补过程种的冗余运算，将S2、r2和S1、r1两部分的计算过程分离，从而有效降低计算量。按照该思路如图1所示，分别视觉重投影误差数据处理加速模块和IMU预积分数据处理加速模块设计硬件结构，实现并行计算以降低数据融合模块加速器运行过程中的时延问题。
如图1所示，发明的数据融合模块加速器的结构包括：由旋转四元数转旋转矩阵加速模块、视觉重投影误差数据处理加速模块、IMU预积分数据处理加速模块、累加模块、片外存储器，其中的视觉重投影误差数据处理加速模块、IMU预积分数据处理加速模块对应上述中分离整合出的S1、r1计算和S2、r2计算，在硬件平台实现并行计算。
图1中箭头方向表示数据流动方向，首先由视觉惯性SLAM前端的视觉惯性里程计VIO得到的旋转四元数、视觉观测信息、先验信息、IMU预积分信息存入片外存储器，旋转四元数转旋转矩阵加速模块采用旋转四元数转旋转矩阵加速策略加速旋转四元数转旋转矩阵得计算，并得到旋转矩阵和旋转逆矩阵，传给视觉重投影误差数据处理加速模块和IMU预积分数据处理加速模块；视觉重投影误差数据处理加速模块基于视觉观测信息、旋转矩阵和旋转逆矩阵采用视觉重投影误差加速策略加速S1和r1的计算；IMU预积分数据处理加速模块基于先验信息、IMU预积分信息、旋转矩阵和旋转逆矩阵采用数据处理加速策略加速S2和r2的计算；S1、r1和S2、r2被传入累加模块执行累加操作，得到线性方程中的S和r存入片外存储器。
2.视觉重投影误差加速策略
图2为视觉重投影海森矩阵和视觉重投影残差矩阵意图，其中视觉重投影海森矩阵由矩阵M、E、C组成，分布位置为：M分布在分块A位置，E分布在分块B和D，C分布在分块F。bc与bm组成视觉重投影残差矩阵，它们在视觉重投影残差矩阵中的分布位置如图2所示。在视觉重投影误差硬件加速模块中，需采用舒尔补消除计算对视觉重投影海森矩阵进行降维，经过舒尔补消除计算的线性方程常被分块成以下增量形式，视觉重投影误差硬件加速模块的输出S1＝(C-EM-1ET)image、r1＝(bc-EM-1bm)image。
(C-EM-1ET)imageΔxc＝(bc-EM-1bm)image
图3为存在共视关系的空间点示意图，对于视觉重投影误差加速策略的实现，首先对相机观测的空间点按是否存在共视关系进行分类，以得到共视点所在关键帧的集合，然后通过遍历这些存在共视关系的空间点及其所在关键帧，计算出对应的雅可比矩阵，进而求得视觉重投影海森矩阵，再进行舒尔补消除计算，输出视觉重投影海森矩阵计算分量S1和视觉重投影残差计算分量r1。令重投影误差关于相机位姿的雅克比矩阵为JT，重投影误差关于空间点的雅克比矩阵为Jx，f(x)为重投影误差，则舒尔补消除计算的增量方程中各参数为：
M＝Jx TJx
E＝JT TJx
bm＝-Jx Tf(x)
bc＝-JT Tf(x)+EM-1bm
C＝JT TJT-EM-1ET
图4为视觉重投影误差加速策略的计算流程，令U表示空间点集合，Vi表示能够观测到第i个空间点的相机采集关键帧集合，即有共视点i的关键帧集合，则有视觉重投影误差加速策略的计算流程：1)遍历U中的空间点；2)参数初始化，包括参数M、E、bm；3)遍历Vi中的关键帧；4)计算雅克比矩阵，并分别计算参数M、E、bm、bc、C；4)利用4)中得到的参数M、E、bm、bc、C执行舒尔补消除计算，得到S1和r1。图4中的循环1为对Vi中的关键帧的遍历，主要用于对参数M、E、bm、bc、C的求解，循环2为对U中空间点的遍历，主要用于执行舒尔补消除计算并得到S1和r1。计算过程中，M、E、bm、bc、C的计算相互不干扰，因此均可以在视觉重投影误差数据处理加速模块中实现并行计算，有效降低运行时的计算时延。此外，由于提前进行了分类操作，在循环2计算S1和r1的过程中参数M、E、bm可复用，这也进一步降低了视觉重投影误差数据处理加速模块运行时的计算量，提升了计算速度。
3.旋转四元数转旋转矩阵加速策略
仅代表旋转的四元数，使用单位旋转四元数q对其进行表示，其中w为实部参数，x、y、z为虚部参数：
q＝w+xi+yj+zk，||q||＝x2+y2+z2+w2＝1
对于给定的一个单位旋转四元数q，可以构造旋转矩阵R(q)：
Figure BDA0003731493400000081
通常对于旋转矩阵求逆操作非常耗费资源，因为对于一般的3*3的旋转矩阵，求解逆矩阵的常用解法是：先求得旋转矩阵所对应的行列式，判断旋转矩阵是否有逆矩阵，然后求取旋转矩阵的伴随矩阵，最后将旋转矩阵的伴随矩阵除以行列式得到旋转矩阵对应的逆矩阵。在这种常用方法的计算步骤下，需要进行大量的乘除计算得到旋转矩阵对应的行列式和伴随矩阵，然后再通过对浮点数的除法得到最后的逆矩阵。由于硬件中对于除法的消耗会大于乘法，因此在旋转四元数转旋转矩阵策略中，通过对旋转四元数的虚部取反实现模块中快速求逆的功能，从而减少中对乘除法的计算。
一个旋转四元数q＝w+xi+yj+zk的共轭为q*＝w-xi-yj-zk，其中用标量向量有序对的形式表示，则旋转四元数q＝[s，v]的共轭为q*＝[s，-v]，其中s＝w，v＝xi+yj+zk。共轭旋转四元数的性质为：
qq*＝[s，v]·[s，-v]＝s2+x2+y2+z2＝||q||2
整理有：
Figure BDA0003731493400000091
由此，对旋转四元数的取反操作在硬件中不需要进行大量乘除计算。
旋转四元数转旋转矩阵加速模块结构如图1所示。在旋转四元数转旋转矩阵策略中，为了实现旋转四元数的旋转矩阵和旋转逆矩阵的并行计算并提升计算速度，在计算旋转逆矩阵时，先对这个旋转四元数的虚部进行取反操作，然后再直接计算取反后的旋转四元数的旋转矩阵即可。为在计算过程中保证数据精度、控制计算规模，使用4个double 64类型共32个字节表示参与计算的旋转四元数。
4.旋转矩阵转旋转四元数加速策略
IMU预积分的雅可比矩阵涉及到旋转矩阵转换成四元数的计算，因此设计旋转矩阵转四元数模块以减少IMU预积分数据处理加速模块中的计算量和调度延迟。
图5是旋转矩阵转旋转四元数策略中的计算流程，为提升数据转换的速度，计算旋转四元数之前需先计算出矩阵的迹，并通过使用快速平方根倒数方法得到计算旋转四元数所需的中间计算参数w。为控制计算规模，使用9个浮点数的数据类型表示旋转矩阵，所用旋转矩阵转化计算方法和快速平方根倒数函数计算方法如下所述。
首先，对于一个合法的旋转矩阵
Figure BDA0003731493400000092
m表示旋转矩阵中的元素，根据旋转矩阵与旋转四元数之间的转换关系：
Figure BDA0003731493400000093
整理可得公式：
m32-m23＝(2yz+2xw)-(2yz-2xw)＝4xw
m13-m31＝(2xz+2yw)-(2xz-2yw)＝4yw
m21-m12＝(2xy+2zw)-(2xy-2zw)＝4zw
旋转矩阵R(q)的迹tr(R(q))＝m11+m22+m33，因此有：
tr(R(q))＝m11+m22+m33
＝3-4(x2+y2+z2)
＝4(1-(x2+y2+z2))-1
＝4w2-1
整理上述所有公式，旋转矩阵R(q)转换后的旋转四元数q的实部参数w与虚部参数x、y、z的计算公式为：
Figure BDA0003731493400000101
Figure BDA0003731493400000102
Figure BDA0003731493400000103
Figure BDA0003731493400000104
q＝w+xi+yj+zk
由于上述公式中w中涉及的平方根运算非常耗时，为了在此阶段可以有效提高运算速度，近一步采用快速平方根倒数函数计算方法，提升旋转矩阵转四元数模块的计算速度，其方法过程为：1)将平方根的倒数转化为求解方程
Figure BDA0003731493400000105
其中a为当前所求平方根倒数的浮点数，x为平方根的倒数值；2)将该方程按牛顿迭代法构造迭代，并在IEEE标准下使用E表示指数，M表示尾数，将旋转矩阵R的平方根倒数表示为
Figure BDA0003731493400000106
形式；3)将
Figure BDA0003731493400000107
在M＝0处展开，忽略二阶及以上高阶项得到
Figure BDA0003731493400000108
4)IEEE数据标准下
Figure BDA0003731493400000109
可通过将R右移一位得到，
Figure BDA00037314934000001010
取为16进制的0x5F3759DF。
5.数据融合模块加速器在算力和功率有限嵌入式平台的部署
本发明选用部署平台为FPGA，型号为ZU9EG。数据融合模块加速器完整部署于FPGA平台，FPGA PL端的计算逻辑用于处理数据融合模块加速器的核心计算。FPGA PL端的互联系统主要负责PS端和PL的数据传递，在Xilinx中主要使用的是AXI4总线协议，其中4条AXI数据总线负责传输数据给数据融合模块加速器，2条AXI数据总线负责将数据传回Arm处理器上，总线之间相互独立，增加了输出传输的带宽，减少了传输时延。互联系统中4个AXI4总线对数据进行并行传输，从而节省数据传输的时间消耗。FPGA平台上的DSP48和LUT资源均得到了充分的利用，分别达到了80.12％和77.03％，而压低的BRAM使用率(32.23％)则在一定程度减小了数据融合模块加速器的实际功耗。
图6为基于数据融合模块加速器的视觉惯性里程计原型机工作流程，IMU传感器和相机采集的IMU数据和图像依次经过VIO前端计算、数据融合模块加速器的处理、VIO后端非线性优化，输出优化后的位置、方向和速度信息。基于数据融合模块加速器的VIO后端优化原型机由CPU负责VIO前端的计算和VIO后端非线性优化中的线性方程求解。基于数据融合模块加速器的视觉惯性里程计原型机在真实场景实验中，可实现超过30FPS的实时运行速率，平均运行速率达到25.1lms/帧，远高于Arm cortex-A53平台的120.36ms/帧，并且能效比仅为0.03J/FPS，与Intel i5-9400F处理器平台的0.6J/FPS相比降低了20倍，这很好的体现了本发明在实际应用中的加速效果和降功耗能力。
本发明未详细公开的部分属于本领域的公知技术。
本领域普通技术人员可以理解上述实施例的各种方法中的全部或部分可以通过程序指令相关硬件来完成，该程序可以存储于一计算机可读存储介质中，存储介质可以包括：只读存储器(ROM，Read Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁盘或光盘等等。
以上所述仅为本发明的实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的保护范围内。

Claims (6)

1.一种数据融合模块加速器，其特征在于包括：旋转四元数转旋转矩阵加速模块、视觉重投影误差数据处理加速模块、IMU预积分数据处理加速模块、累加模块、片外存储器；
片外存储器，用于存储通过视觉惯性SLAM视觉惯性里程计VIO前端得到的旋转四元数、视觉观测信息、先验信息和IMU预积分信息；所述视觉观测信息包括不同相机观测的空间点集、相机采集的关键帧集、关键帧特征点集和特征点匹配点集；所述IMU预积分信息包括位置信息、方向信息和速度信息；
旋转四元数转旋转矩阵加速模块，包括旋转四元数转旋转矩阵加速策略；片外存储器中的旋转四元数传入旋转四元数转旋转矩阵加速模块，利用旋转四元数转旋转矩阵加速策略加速旋转矩阵和旋转逆矩阵的计算；所得旋转矩阵和旋转逆矩阵被传入视觉重投影误差加速模块和IMU预积分数据处理加速模块；
视觉重投影误差加速模块包含视觉重投影误差加速策略；片外存储器中的视觉观测信息、旋转四元数转旋转矩阵加速模块输出的旋转矩阵和旋转逆矩阵传入视觉重投影误差加速模块，依次进行视觉重投影海森矩阵计算、舒尔补消除计算，计算过程中利用视觉重投影误差加速策略提升舒尔补消除计算速度；所得视觉重投影海森矩阵计算分量S1和视觉重投影残差计算分量r1被传入累加模块；
IMU预积分数据处理加速模块包含数据处理加速策略；片外存储器中的先验信息、IMU预积分信息和和旋转四元数转旋转矩阵加速模块输出的旋转矩阵和旋转逆矩阵传入IMU预积分数据处理加速模块进行滑动窗口先验海森矩阵计算、IMU预积分海森矩阵计算，计算过程中利用数据处理加速策略提升IMU预积分海森矩阵计算中涉及的旋转矩阵转旋转四元数的计算速度，并限制滑动窗口先验海森矩阵计算的规模；所得滑动窗口先验海森矩阵和IMU预积分海森矩阵的整合计算分量S2、滑动窗口先验残差和IMU预积分残差的整合计算分量r2被传入累加模块；
累加模块基于传入的视觉重投影海森矩阵计算分量S1、视觉重投影残差计算分量r1、IMU预积分海森矩阵的整合计算分量S2、IMU预积分残差的整合计算分量r2执行累加操作，得到的VIO后端优化海森矩阵计算分量S和VIO后端优化残差计算分量r传入片外存储器；片外存储器中的S和r用于后续的VIO后端非线性优化的计算。
2.根据权利要求1所述数据融合模块加速器，其特征在于：所述旋转四元数转旋转矩阵加速模块中，旋转四元数转旋转矩阵加速策略包括四个部分：第一，使用4个double 64类型的数据表示旋转四元数,共32个字节；第二，旋转四元数转换为旋转矩阵的过程为：根据输入的旋转四元数直接计算其对应的旋转矩阵；第三，旋转四元数转换为旋转逆矩阵的过程为：先将输入的旋转四元数执行虚部取反，然后计算取反后的旋转矩阵；第四，将第二与第三在四元数转旋转矩阵模块中并行处理以提高计算速度。
3.根据权利要求1所述数据融合模块加速器，其特征在于：所述视觉重投影误差加速模块中，视觉重投影误差数据处理加速策略；
所述视觉重投影误差数据处理加速策略用于简化舒尔补消除计算的复杂度，以加速S1和r1的计算，方法为取视觉重投影海森矩阵上对应位置存在共视关系的特征点数据，执行舒尔补消除计算；所述公视关系为不同相机在同一时间观测到的相同空间点。
4.根据权利要求1所述的数据融合模块加速器，其特征在于：所述IMU预积分数据处理加速模块中，数据处理加速策略包括滑动窗口长度设置、旋转矩阵转旋转四元数加速策略；
所述滑动窗口长度设置，用于限制滑动窗口先验海森矩阵的计算规模；
所述旋转矩阵转旋转四元数加速策略，用于加速旋转矩阵转旋转四元数计算，包括：第一，使用长度为9个浮点数类型的数据表示旋转矩阵；第二，通过求解旋转矩阵的迹和使用快速平方根倒数方法，以少量精度损失为代价提高求解旋转矩阵对应旋转四元数的计算速度。
5.根据权利要求1所述数据融合模块加速器，其特征在于：所述数据融合模块加速器基于FPGA平台实现，旋转四元数转旋转矩阵加速模块、视觉重投影误差数据处理加速模块、IMU预积分数据处理加速模块、累加模块均部署于FPGA平台的PL端，使用AXI4总线协议，与片外存储器通过4条AXI数据总线连接以提升数据传输的吞吐量。
6.一种基于权利要求1-5任意之一所述加速器的数据融合模块加速方法，其特征在于实现步骤如下：
步骤1：通过视觉惯性SLAM视觉惯性里程计VIO前端所得旋转四元数、视觉观测信息、先验信息、IMU预积分信息存入片外存储器；
步骤2：基于步骤1中的旋转四元数，旋转四元数转旋转矩阵加速模块采用旋转四元数转旋转矩阵加速策略实现计算加速，得到旋转矩阵和旋转逆矩阵；
步骤3：基于步骤1中的视觉观测信息、步骤2得到的旋转矩阵和旋转逆矩阵，视觉重投影误差加速模块采用视觉重投影误差加速策略，实现对视觉重投影海森矩阵的舒尔补消除计算的加速，得到视觉重投影雅可比矩阵、视觉重投影海森矩阵、视觉重投影海森矩阵计算分量S1和视觉重投影残差计算分量r1；
步骤4：基于步骤1中的先验信息、IMU预积分信息、步骤2得到的旋转矩阵和旋转逆矩阵，IMU预积分数据处理加速模块采用数据处理加速策略，实现滑动窗口先验海森矩阵和IMU预积分海森矩阵计算的加速，得到滑动窗口先验海森矩阵、IMU预积分海森矩阵、IMU预积分雅可比矩阵、IMU预积分和滑动窗口先验相关的海森矩阵计算分量S2、IMU预积分和滑动窗口先验相关的残差计算分量r2；
步骤5：基于步骤3和步骤4输出的S1、r1、S2、r2执行累加操作，得到累加后海森矩阵计算分量S和累加后残差计算分量r存入片外存储器；
步骤6：步骤5中所得S和r用于后续的VIO后端非线性优化的计算。
CN202210784735.2A 2022-07-05 2022-07-05 一种数据融合模块加速器及方法 Pending CN115168283A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210784735.2A CN115168283A (zh) 2022-07-05 2022-07-05 一种数据融合模块加速器及方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210784735.2A CN115168283A (zh) 2022-07-05 2022-07-05 一种数据融合模块加速器及方法

Publications (1)

Publication Number Publication Date
CN115168283A true CN115168283A (zh) 2022-10-11

Family

ID=83491127

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210784735.2A Pending CN115168283A (zh) 2022-07-05 2022-07-05 一种数据融合模块加速器及方法

Country Status (1)

Country Link
CN (1) CN115168283A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116683546A (zh) * 2023-08-03 2023-09-01 中国电建集团华东勘测设计研究院有限公司 不依赖于风速测量的风力发电机组功率备用控制方法
CN117705107A (zh) * 2024-02-06 2024-03-15 电子科技大学 基于两阶段稀疏舒尔补的面向视觉惯性定位方法

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116683546A (zh) * 2023-08-03 2023-09-01 中国电建集团华东勘测设计研究院有限公司 不依赖于风速测量的风力发电机组功率备用控制方法
CN117705107A (zh) * 2024-02-06 2024-03-15 电子科技大学 基于两阶段稀疏舒尔补的面向视觉惯性定位方法
CN117705107B (zh) * 2024-02-06 2024-04-16 电子科技大学 基于两阶段稀疏舒尔补的面向视觉惯性定位方法

Similar Documents

Publication Publication Date Title
CN115168283A (zh) 一种数据融合模块加速器及方法
CN108446761B (zh) 一种神经网络加速器及数据处理方法
Tang et al. 1-bit adam: Communication efficient large-scale training with adam’s convergence speed
CN111459877B (zh) 基于FPGA加速的Winograd YOLOv2目标检测模型方法
CN114756383A (zh) 一种分布式计算方法、系统、设备及存储介质
CN111898733B (zh) 一种深度可分离卷积神经网络加速器架构
Huo et al. Asynchronous stochastic gradient descent with variance reduction for non-convex optimization
CN114218875A (zh) 一种用于流场预测的加速方法及装置
CN109657794B (zh) 一种基于指令队列的分布式深度神经网络性能建模方法
CN111985064A (zh) 一种永磁电机的代理辅助优化设计方法及系统
CN111899280A (zh) 采用深度学习和混合型位姿估计的单目视觉里程计方法
CN112380793B (zh) 基于gpu的湍流燃烧数值模拟并行加速实现方法
CN106408653A (zh) 一种面向大规模三维重建的实时鲁棒的集束调整方法
CN111640296B (zh) 交通流预测方法、系统、存储介质及终端
CN115861646A (zh) 一种基于结构重参数化的轻量级目标检测方法及系统
CN111831354A (zh) 数据精度配置方法、装置、芯片、芯片阵列、设备及介质
CN114461978A (zh) 数据处理方法、装置、电子设备及可读存储介质
CN111091183A (zh) 一种神经网络加速系统和方法
Wang et al. GPU-based power flow analysis with continuous Newton's method
CN111028125B (zh) 一种用于slam的已知自身位姿的光束平差法fpga加速器
CN108090865B (zh) 光学卫星遥感影像在轨实时流式处理方法及系统
CN115935888A (zh) 一种神经网络加速系统
CN106055890B (zh) 一种使用gpu加速lcs计算的方法
CN104897176A (zh) 一种多核并行摄影测量区域网平差方法
CN115293978A (zh) 卷积运算电路和方法、图像处理设备

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination