[發(fā)明專利]移動(dòng)機(jī)器人初始位姿估計(jì)方法、系統(tǒng)、計(jì)算機(jī)及機(jī)器人

在審

• 申請(qǐng)?zhí)枺?/td>: 202210177620.7
• 申請(qǐng)日: 2022-02-24
• 公開（公告）號(hào): CN114603555A
• 公開（公告）日: 2022-06-10
• 發(fā)明（設(shè)計(jì)）人: 聶志華;鄭友勝;趙迪;陳韜;曾競(jìng)凱;黃路
• 申請(qǐng)（專利權(quán)）人: 江西省智能產(chǎn)業(yè)技術(shù)創(chuàng)新研究院
• 主分類號(hào): B25J9/16
• 分類號(hào): B25J9/16;G01C21/00;G06T7/73;G06K9/62;G06V10/40;G06V30/18;G06F17/16
• 代理公司: 北京清亦華知識(shí)產(chǎn)權(quán)代理事務(wù)所(普通合伙) 11201
• 代理人: 何世磊

本發(fā)明提供了一種移動(dòng)機(jī)器人初始位姿估計(jì)方法、系統(tǒng)、計(jì)算機(jī)及機(jī)器人，該方法包括啟用相機(jī)并通過相機(jī)發(fā)布RGB圖像流話題節(jié)點(diǎn)；獲取話題節(jié)點(diǎn)中的圖像，并計(jì)算出圖像中的ORB特征的第一關(guān)鍵點(diǎn)與第一描述子；讀取模板圖像集并加載出實(shí)驗(yàn)區(qū)圖像詞典；計(jì)算出關(guān)鍵點(diǎn)集與描述子集；根據(jù)實(shí)驗(yàn)區(qū)圖像詞典將第一描述子和第二描述子均轉(zhuǎn)換成對(duì)應(yīng)的特征向量，并計(jì)算出相似度得分；從模板圖像集中提取出匹配度最高的圖像及對(duì)應(yīng)的位姿，并計(jì)算出匹配度最高的圖像的旋轉(zhuǎn)矩陣與平移矩陣；根據(jù)匹配度最高的位姿計(jì)算出當(dāng)前位置的空間姿態(tài)。本申請(qǐng)能夠較短時(shí)間在已知占用地圖區(qū)域內(nèi)有效估計(jì)移動(dòng)機(jī)器人位姿，并且對(duì)算法的算力要求低。

技術(shù)領(lǐng)域

本發(fā)明涉及移動(dòng)機(jī)器人技術(shù)領(lǐng)域，特別涉及一種移動(dòng)機(jī)器人初始位姿估計(jì)方法、系統(tǒng)、計(jì)算機(jī)及機(jī)器人。

背景技術(shù)

現(xiàn)有的基于ROS navigation功能包來實(shí)現(xiàn)導(dǎo)航的移動(dòng)機(jī)器人大多數(shù)使用AMCL(Adaptive Monte Carlo Localization)算法進(jìn)行粒子濾波，從而對(duì)自身在已知的地圖中進(jìn)行概率定位。現(xiàn)有的移動(dòng)機(jī)器人在首次啟動(dòng)時(shí)，通常要求用戶手動(dòng)輸入當(dāng)前機(jī)器人的初始化位姿，然而，用戶手動(dòng)輸入的初始化位姿不夠準(zhǔn)確，且機(jī)器人不移動(dòng)時(shí)也不會(huì)自動(dòng)更新初始化位姿。

現(xiàn)有技術(shù)大部分通過將上述AMCL粒子分布至整張地圖、以及強(qiáng)制粒子更新等手段來對(duì)移動(dòng)機(jī)器人的初始姿態(tài)進(jìn)行估計(jì)，然而，通過將AMCL粒子分布至整張地圖得出的移動(dòng)機(jī)器人的初始位姿的精度較差，并且強(qiáng)制粒子更新會(huì)降低當(dāng)前移動(dòng)機(jī)器人后續(xù)定位的精度。

因此，針對(duì)現(xiàn)有技術(shù)的不足，提供一種能夠提升移動(dòng)機(jī)器人初始位姿估計(jì)精度的方法很有必要。

發(fā)明內(nèi)容

基于此，本發(fā)明的目的是提供一種移動(dòng)機(jī)器人初始位姿估計(jì)方法、系統(tǒng)、計(jì)算機(jī)及機(jī)器人，以實(shí)現(xiàn)移動(dòng)機(jī)器人初始位姿估計(jì)。

本發(fā)明實(shí)施例第一方面提出了一種移動(dòng)機(jī)器人初始位姿估計(jì)方法，所述方法包括：

當(dāng)初始化ROS完成時(shí)，啟用相機(jī)并通過所述相機(jī)發(fā)布RGB圖像流話題節(jié)點(diǎn)；

獲取所述RGB圖像流話題節(jié)點(diǎn)中的圖像，并計(jì)算出所述圖像中的ORB特征的第一關(guān)鍵點(diǎn)與第一描述子；

讀取模板圖像集并加載出實(shí)驗(yàn)區(qū)圖像詞典；

計(jì)算出所述模板圖像集中的ORB特征的關(guān)鍵點(diǎn)集與描述子集，所述描述子集包括若干第二描述子，所述關(guān)鍵點(diǎn)集包括若干第二關(guān)鍵點(diǎn)；

根據(jù)所述實(shí)驗(yàn)區(qū)圖像詞典將所述第一描述子和所述第二描述子均轉(zhuǎn)換成對(duì)應(yīng)的特征向量，并計(jì)算出所述特征向量之間的相似度得分；

從所述模板圖像集中提取出匹配度最高的圖像及對(duì)應(yīng)的位姿，并計(jì)算出所述相機(jī)獲取的圖像與所述模板圖像集中的匹配度最高的圖像的旋轉(zhuǎn)矩陣與平移矩陣；

根據(jù)匹配度最高的圖像對(duì)應(yīng)的位姿計(jì)算出當(dāng)前位置的空間姿態(tài)，并發(fā)布ROS初始化位置消息。

本發(fā)明的有益效果是：本申請(qǐng)首次提出使用視覺圖像信息來輔助移動(dòng)機(jī)器人開展初始位姿估計(jì)，具體的，通過視覺SLAM技術(shù)獲取先驗(yàn)關(guān)鍵幀及其姿態(tài)信息，基于視覺詞典的回環(huán)檢測(cè)技術(shù)匹配相似度最高模板圖像，根據(jù)對(duì)極約束使用八點(diǎn)法求解當(dāng)前幀與模板幀變換矩陣，與先驗(yàn)姿態(tài)處理后得移動(dòng)機(jī)器人位姿估計(jì)。算法能夠在較短時(shí)間內(nèi)部署在基于ROS控制的移動(dòng)機(jī)器人上，于已知占用地圖區(qū)域內(nèi)可有效估計(jì)移動(dòng)機(jī)器人位姿，并且對(duì)算法的算力要求低，適用于大范圍的推廣與使用。

優(yōu)選的，所述獲取所述RGB圖像流話題節(jié)點(diǎn)中的圖像，并計(jì)算出所述圖像中的ORB特征的第一關(guān)鍵點(diǎn)與第一描述子的步驟包括：

基于linux/ubuntu操作系統(tǒng)環(huán)境，通過cv::bridge在所述RGB圖像流話題節(jié)點(diǎn)中獲取到移動(dòng)機(jī)器人當(dāng)前位置的正方向；