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

有效

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

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號塊矩陣中的相應元素。