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

说明书

一种面向机器人EKF‑SLAM算法的加速方法，属于面向机器人环境地图构建及定位基础的EKF‑SLAM算法加速领域。解决了现有EKF‑SLAM算法运算速度慢的问题。本发明加速方法是基于FPGA实现的，通过FPGA对EKF‑SLAM算法中的矩阵(Z·K^T)^T·K^T进行并行运算，从而实现对EKF‑SLAM算法的加速。本发明主要用于对EKF‑SLAM算法进行加速。

技术领域

本发明属于面向机器人环境地图构建及定位基础的EKF-SLAM算法加速领域，特别涉及一种借助现场可编程门阵列(Field Programmable Gate Array，FPGA)实现EKF-SLAM算法硬件加速的方法。

背景技术

面向机器人环境地图构建及定位的基础是采用EKF-SLAM算法实现，而对于EKF-SLAM算法，其过程可分为两个部分：

预测循环：

更新循环：

式中，表示状态向量的先验；f表示状态方程；表示状态向量的后验；u_k表示输入的控制向量；表示协方差矩阵的先验；F表示状态方程对状态向量的偏导数；表示协方差矩阵的后验；R_k表示状态向量噪声的协方差矩阵；Z表示新息协方差矩阵；H表示观测方程对状态向量的偏导数；Q_k表示观测向量噪声的协方差矩阵；K表示卡尔曼增益矩阵；z_k表示观测向量；h表示观测方程；

研究表明：更新循环的最后一个公式中(Z·K^T)^T·K^T占EKF-SLAM算法中所有方程所用时间的64％，而EKF-SLAM算法的全部计算过程是在CPU中进行的，矩阵Z的维数为2×2、矩阵K的维数为2×n，且在计算(Z·K^T)^T·K^T的过程中，采用串行计算的计算方式，每次计算可获得结果矩阵中的一个元素，计算速度慢，又由于(Z·K^T)^T·K^T的运算过程慢，导致EKF-SLAM算法的运算速度慢，因此，如何对EKF-SLAM算法进行加速，以上问题亟需解决。

发明内容

本发明目的是为了解决现有EKF-SLAM算法运算速度慢的问题，本发明提供了一种面向机器人EKF-SLAM算法的加速方法。

一种面向机器人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；