一种面向机器人EKF-SLAM算法的加速方法技术

一种面向机器人EKF‑SLAM算法的加速方法，属于面向机器人环境地图构建及定位基础的EKF‑SLAM算法加速领域。解决了现有EKF‑SLAM算法运算速度慢的问题。本发明专利技术加速方法是基于FPGA实现的，通过FPGA对EKF‑SLAM算法中的矩阵(Z·K

【技术实现步骤摘要】
一种面向机器人EKF-SLAM算法的加速方法
本专利技术属于面向机器人环境地图构建及定位基础的EKF-SLAM算法加速领域，特别涉及一种借助现场可编程门阵列(FieldProgrammableGateArray，FPGA)实现EKF-SLAM算法硬件加速的方法。
技术介绍
面向机器人环境地图构建及定位的基础是采用EKF-SLAM算法实现，而对于EKF-SLAM算法，其过程可分为两个部分：预测循环：更新循环：式中，表示状态向量的先验；f表示状态方程；表示状态向量的后验；uk表示输入的控制向量；表示协方差矩阵的先验；F表示状态方程对状态向量的偏导数；表示协方差矩阵的后验；Rk表示状态向量噪声的协方差矩阵；Z表示新息协方差矩阵；H表示观测方程对状态向量的偏导数；Qk表示观测向量噪声的协方差矩阵；K表示卡尔曼增益矩阵；zk表示观测向量；h表示观测方程；研究表明：更新循环的最后一个公式中(Z·KT)T·KT占EKF-SLAM算法中所有方程所用时间的64％，而EKF-SLAM算法的全部计算过程是在CPU中进行的，矩阵Z的维数为2×2、矩阵K的维数为2×n，且在计算(Z·KT)T·KT的过程中，采用串行计算的计算方式，每次计算可获得结果矩阵中的一个元素，计算速度慢，又由于(Z·KT)T·KT的运算过程慢，导致EKF-SLAM算法的运算速度慢，因此，如何对EKF-SLAM算法进行加速，以上问题亟需解决。
技术实现思路
本专利技术目的是为了解决现有EKF-SLAM算法运算速度慢的问题，本专利技术提供了一种面向机器人EKF-SLAM算法的加速方法。一种面向机器人EKF-SLAM算法的加速方法，该加速方法是基于FPGA实现的，通过FPGA对EKF-SLAM算法中的矩阵(Z·KT)T·KT进行并行运算，从而实现对EKF-SLAM算法的加速；其中，矩阵Z的维数为2×2，矩阵K的维数为n×2，n为整数。优选的是，加速方法的具体过程为：步骤一、将机器人DDR内存中矩阵K的转置KT划分成M个2×2的1号块矩阵，按照划分的次序分别定义为第一至第M个1号块矩阵，其中，DDR内存中还存储有矩阵Z，M为整数，n＝2M；步骤二、FPGA从DDR内存中依次调取第一至第M个1号块矩阵和矩阵Z，并进行存储；按照M个1号块矩阵的调取顺序，使矩阵Z分别与KT中M个1号块矩阵相乘，从而获得矩阵Z·KT；其中，矩阵Z与任意一个1号块矩阵相乘的实现方式为：矩阵Z中的4个元素以并行运算的方式，与所述任意一个1号块矩阵中的4个元素进行乘法运算；步骤三、对矩阵Z·KT进行转置，获得矩阵(Z·KT)T，并将矩阵(Z·KT)T划分成M个2×2的2号块矩阵，按照划分的次序分别定义为第一至第M个2号块矩阵；步骤四、使矩阵(Z·KT)T中第一至第M个2号块矩阵分别与KT中的第一至第M个1号块矩阵相乘，从而获得矩阵(Z·KT)T·KT，完成对EKF-SLAM算法的加速；其中，M个2号块矩阵中任意的一个2号块矩阵与相应的1号块矩阵相乘的实现方式为：任意的一个2号块矩阵中的4个元素以并行运算的方式，与相应的1号块矩阵中的4个元素进行乘法运算。优选的是，FPGA与机器人DDR内存之间通过AXI协议的接口模块进行通信。优选的是，FPGA包括8个双口RAM模块、4个寄存器和两个处理模块；其中，4个寄存器分别用于存储矩阵Z中的4个元素；4个双口RAM模块作为一组，且分别定义为第一至第四个双口RAM模块；第一个双口RAM模块用于存储M个1号块矩阵中第一行第一列的元素；第二个双口RAM模块用于存储M个1号块矩阵中第一行第二列的元素；第三个双口RAM模块用于存储M个1号块矩阵中第二行第一列的元素；第四个双口RAM模块用于存储M个1号块矩阵中第二行第二列的元素；剩余4个双口RAM模块作为另一组，且分别定义为第五至第八个双口RAM模块；第五个双口RAM模块用于存储M个2号块矩阵中第一行第一列的元素；第六个双口RAM模块用于存储M个2号块矩阵中第一行第二列的元素；第七个双口RAM模块用于存储M个2号块矩阵中第二行第一列的元素；第八个双口RAM模块用于存储M个2号块矩阵中第二行第二列的元素；两个处理模块中的第一个处理模块用于实现矩阵Z分别与KT中M个1号块矩阵的相乘运算；第二个处理模块，用于实现对矩阵Z·KT进行转置，获得矩阵(Z·KT)T，并将矩阵(Z·KT)T划分成M个2×2的2号块矩阵；还用于实现矩阵(Z·KT)T中第一至第M个2号块矩阵分别与KT中的第一至第M个1号块矩阵的相乘运算。优选的是，通过改变第一至第四个双口RAM模块的地址线，可使第一至第四个双口RAM模块均可依次输出第一至M个1号块矩阵中的相应元素。优选的是，通过改变第五至第八个双口RAM模块的地址线，可使第五至第八个双口RAM模块均可依次输出第一至M个2号块矩阵中的相应元素。优选的是，矩阵(Z·KT)T·KT为由M×M个块矩阵构成，且该块矩阵的维数为2×2。本专利技术带来的有益效果为：通过借助FPGA，使EKF-SLAM算法中的矩阵(Z·KT)T·KT的运算过程在FPGA内实现，且以并行的运算方式进行运算，从而提高运算速度，满足EKF-SLAM算法对实时性的要求，本专利技术脱离了现有技术中EKF-SLAM算法完全依赖于CPU实现的方式，避免占用机器人的硬件存储资源及增加CPU的计算量的缺陷。本专利技术对EKF-SLAM算法进行加速的过程中包括两轮加速计算：第一轮加速是对于矩阵Z·KT来说，加速的实现方式为，是将KT划分成M个2×2的1号块矩阵，并按照M个1号块矩阵的调取顺序，使矩阵Z分别与KT中M个1号块矩阵相乘，每一次相乘运算都能进行并行运算，第一轮加速过程中，先将维数较大的矩阵划分成维数较小的块矩阵，利用较小的块矩阵进行并行运算，从而提高第一轮的运算速度。第二轮加速是对于(Z·KT)T·KT来说，加速的实现方式为，将第一轮加速后的矩阵(Z·KT)T划分成M个具有较小维数的2号块矩阵，M个2号块矩阵分别与1号块矩阵进行并行相乘运算，每一次相乘运算都能进行并行运算，从而实现第二轮加速。附图说明图1是本专利技术所述FPGA的原理示意图。具体实施方式下面将结合本专利技术实施例中的附图，对本专利技术实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本专利技术一部分实施例，而不是全部的实施例。基于本专利技术中的实施例，本领域普通技术人员在没有作出创造性劳动的前提下所获得的所有其他实施例，都属于本专利技术保护的范围。需要说明的是，在不冲突的情况下，本专利技术中的实施例及实施例中的特征可以相互组合。参见图1说明本实施方式，本实施方式所述的一种面向机器人EKF-SLAM算法的加速方法，该加速方法是基于FPGA实现的，通过FPGA对EKF-SLAM算法中的矩阵(Z·KT)本文档来自技高网...

【技术保护点】
1.一种面向机器人EKF-SLAM算法的加速方法，其特征在于，该加速方法是基于FPGA实现的，通过FPGA对EKF-SLAM算法中的矩阵(Z·K

【技术特征摘要】
1.一种面向机器人EKF-SLAM算法的加速方法，其特征在于，该加速方法是基于FPGA实现的，通过FPGA对EKF-SLAM算法中的矩阵(Z·KT)T·KT进行并行运算，从而实现对EKF-SLAM算法的加速；
其中，矩阵Z的维数为2×2，矩阵K的维数为n×2，n为整数。


2.根据权利要求1所述的一种面向机器人EKF-SLAM算法的加速方法，其特征在于，加速方法的具体过程为：
步骤一、将机器人DDR内存中矩阵K的转置KT划分成M个2×2的1号块矩阵，按照划分的次序分别定义为第一至第M个1号块矩阵，其中，DDR内存中还存储有矩阵Z，M为整数，n＝2M；
步骤二、FPGA从DDR内存中依次调取第一至第M个1号块矩阵和矩阵Z，并进行存储；按照M个1号块矩阵的调取顺序，使矩阵Z分别与KT中M个1号块矩阵相乘，从而获得矩阵Z·KT；
其中，矩阵Z与任意一个1号块矩阵相乘的实现方式为：矩阵Z中的4个元素以并行运算的方式，与所述任意一个1号块矩阵中的4个元素进行乘法运算；
步骤三、对矩阵Z·KT进行转置，获得矩阵(Z·KT)T，并将矩阵(Z·KT)T划分成M个2×2的2号块矩阵，按照划分的次序分别定义为第一至第M个2号块矩阵；
步骤四、使矩阵(Z·KT)T中第一至第M个2号块矩阵分别与KT中的第一至第M个1号块矩阵相乘，从而获得矩阵(Z·KT)T·KT，完成对EKF-SLAM算法的加速；
其中，M个2号块矩阵中任意的一个2号块矩阵与相应的1号块矩阵相乘的实现方式为：任意的一个2号块矩阵中的4个元素以并行运算的方式，与相应的1号块矩阵中的4个元素进行乘法运算。


3.根据权利要求1所述的一种面向机器人EKF-SLAM算法的加速方法，其特征在于，FPGA与机器人DDR内存之间通过AXI协议的接口模块进行通信。


4.根据权利要求1所述的一种面向机器人EKF-SLAM算法的加速方法，其特征在于，FPGA包括8个...

【专利技术属性】
技术研发人员：王珂，包敏杰，李瑞峰，赵立军，
申请(专利权)人：哈尔滨工业大学，
类型：发明
国别省市：黑龙江;23