[发明专利]一种面向机器人EKF-SLAM算法的加速方法

权利要求书

1.一种面向机器人EKF-SLAM算法的加速方法，其特征在于，该加速方法是基于FPGA实现的，通过FPGA对EKF-SLAM算法中的矩阵(Z·K^T)^T·K^T进行并行运算，从而实现对EKF-SLAM算法的加速；

其中，矩阵Z的维数为2×2，矩阵K的维数为n×2，n为整数；

加速方法的具体过程为：

步骤一、将机器人DDR内存中矩阵K的转置K^T划分成M个2×2的1号块矩阵，按照划分的次序分别定义为第一至第M个1号块矩阵，其中，DDR内存中还存储有矩阵Z，M为整数，n＝2M；

步骤二、FPGA从DDR内存中依次调取第一至第M个1号块矩阵和矩阵Z，并进行存储；按照M个1号块矩阵的调取顺序，使矩阵Z分别与K^T中M个1号块矩阵相乘，从而获得矩阵Z·K^T；

其中，矩阵Z与任意一个1号块矩阵相乘的实现方式为：矩阵Z中的4个元素以并行运算的方式，与所述任意一个1号块矩阵中的4个元素进行乘法运算；

步骤三、对矩阵Z·K^T进行转置，获得矩阵(Z·K^T)^T，并将矩阵(Z·K^T)^T划分成M个2×2的2号块矩阵，按照划分的次序分别定义为第一至第M个2号块矩阵；

步骤四、使矩阵(Z·K^T)^T中第一至第M个2号块矩阵分别与K^T中的第一至第M个1号块矩阵相乘，从而获得矩阵(Z·K^T)^T·K^T，完成对EKF-SLAM算法的加速；

其中，M个2号块矩阵中任意的一个2号块矩阵与相应的1号块矩阵相乘的实现方式为：任意的一个2号块矩阵中的4个元素以并行运算的方式，与相应的1号块矩阵中的4个元素进行乘法运算。

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

3.根据权利要求1所述的一种面向机器人EKF-SLAM算法的加速方法，其特征在于，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分别与K^T中M个1号块矩阵的相乘运算；

第二个处理模块，用于实现对矩阵Z·K^T进行转置，获得矩阵(Z·K^T)^T，并将矩阵(Z·K^T)^T划分成M个2×2的2号块矩阵；还用于实现矩阵(Z·K^T)^T中第一至第M个2号块矩阵分别与K^T中的第一至第M个1号块矩阵的相乘运算。

4.根据权利要求3所述的一种面向机器人EKF-SLAM算法的加速方法，其特征在于，通过改变第一至第四个双口RAM模块的地址线，可使第一至第四个双口RAM模块均可依次输出第一至M个1号块矩阵中的相应元素。