[發明專利]一種面向機器人EKF-SLAM算法的加速方法

有效

• 申請號: 202010718538.1
• 申請日: 2020-07-23
• 公開（公告）號: CN111722834B
• 公開（公告）日: 2021-12-28
• 發明（設計）人: 王珂;包敏杰;李瑞峰;趙立軍
• 申請（專利權）人: 哈爾濱工業大學
• 主分類號: G06F7/78
• 分類號: G06F7/78;G06F15/78;G06K9/00;G06T11/20;G06T17/05
• 代理公司: 哈爾濱市松花江專利商標事務所 23109
• 代理人: 董玉嬌

一種面向機器人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；