[發(fā)明專(zhuān)利]一種基于智能機(jī)器人地面特征的PTAM改進(jìn)方法

有效

• 申請(qǐng)?zhí)枺?/td>: 201510025810.7
• 申請(qǐng)日: 2015-01-19
• 公開(kāi)（公告）號(hào): CN104732518B
• 公開(kāi)（公告）日: 2017-09-01
• 發(fā)明（設(shè)計(jì)）人: 賈松敏;王可;宣璇;張鵬;董政胤
• 申請(qǐng)（專(zhuān)利權(quán)）人: 北京工業(yè)大學(xué)
• 主分類(lèi)號(hào): G06T7/80
• 分類(lèi)號(hào): G06T7/80;G01C21/00
• 代理公司: 北京思海天達(dá)知識(shí)產(chǎn)權(quán)代理有限公司11203
• 代理人: 沈波

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

本發(fā)明屬于機(jī)器人視覺(jué)領(lǐng)域，涉及一種基于地面特征的PTAM算法改進(jìn)。

背景技術(shù)

隨著機(jī)器人與人類(lèi)的關(guān)系越來(lái)越緊密，智能機(jī)器人的相關(guān)技術(shù)受到了極大的重視。移動(dòng)機(jī)器人的同步定位與建圖(Simultaneous Localization and Mapping,SLAM)是當(dāng)前智能移動(dòng)機(jī)器人最主流的定位技術(shù)之一。它實(shí)際上就是一個(gè)運(yùn)動(dòng)估計(jì)問(wèn)題，利用傳感器獲取的內(nèi)部和外部數(shù)據(jù)，計(jì)算出移動(dòng)機(jī)器人在某一時(shí)刻的位置，同時(shí)建立所依賴(lài)的地圖模型。基于視覺(jué)的SLAM是屬于視覺(jué)測(cè)量的研究范疇，由于視覺(jué)傳感器擁有自身獨(dú)特的優(yōu)點(diǎn)：體積小，重量輕，價(jià)格便宜，安裝方便，提取的外部信息非常豐富。這些優(yōu)勢(shì)進(jìn)一步促進(jìn)了當(dāng)前基于視覺(jué)的SLAM的研究和應(yīng)用。申請(qǐng)?zhí)枮?01310678018.2提供了一種采用SURF作為單目視覺(jué)SLAM的特征檢測(cè)算子，分別從感興趣點(diǎn)的檢測(cè)、SURF描述子的生成及SURF點(diǎn)匹配等三個(gè)方面進(jìn)行了創(chuàng)新和改進(jìn)，提出了基于區(qū)域增長(zhǎng)的SURF特征匹配方法完成機(jī)器人的同時(shí)定位與地圖創(chuàng)建，使得在SURF描述子匹配過(guò)程中，某一描述子僅與最可能與之匹配的描述子進(jìn)行比對(duì)，從而顯著的減少了單目視覺(jué)SLAM問(wèn)題中的比對(duì)次數(shù)，提高了匹配速度。但該方法依然存在不能建立米制地圖以及無(wú)法提供三維特征信息的問(wèn)題，算法魯棒性與實(shí)時(shí)性有待進(jìn)一步提高。

從運(yùn)動(dòng)中創(chuàng)建環(huán)境模型技術(shù)是一種同時(shí)求解攝像機(jī)運(yùn)動(dòng)軌跡和場(chǎng)景結(jié)構(gòu)模型的方法。作為SFM的代表性方法，G.Klein等人于2007年在增強(qiáng)現(xiàn)實(shí)(AR)領(lǐng)域首先提出了PTAM(Parallel Tracking and Mapping)的概念，用來(lái)實(shí)時(shí)地解決環(huán)境識(shí)別問(wèn)題。但未改進(jìn)的PTAM算法也存在無(wú)法建立米制地圖的問(wèn)題，同時(shí)對(duì)相機(jī)的移動(dòng)有著嚴(yán)格限制。

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

針對(duì)以上存在的問(wèn)題，本發(fā)明提出了一種基于地面特征的PTAM改進(jìn)算法，在加入地面特征的基礎(chǔ)上，PTAM對(duì)單目攝像機(jī)的當(dāng)前位姿與三維空間點(diǎn)進(jìn)行同步估計(jì)，在利用FAST角點(diǎn)方法更新檢測(cè)特征點(diǎn)的同時(shí)，用最優(yōu)的局部與全局光束平差法，不斷地更新攝像機(jī)與三維特征點(diǎn)的位姿，完成了高精度定位與地圖創(chuàng)建，該發(fā)明有效解決了無(wú)法創(chuàng)建米制地圖的問(wèn)題，同時(shí)在實(shí)時(shí)性及三維特征提取上有著極大的提高，對(duì)于解決機(jī)器人導(dǎo)航定位、三維重建中的相關(guān)問(wèn)題有著重要意義。

本發(fā)明采用如下的技術(shù)方案：

首先，完成參數(shù)校正，這包括參數(shù)定義與相機(jī)校正；接著對(duì)關(guān)鍵幀中的特征進(jìn)行跟蹤，再實(shí)現(xiàn)位姿的初始化，再此基礎(chǔ)上對(duì)平面參數(shù)進(jìn)行估計(jì)；然后利用相機(jī)獲取當(dāng)前環(huán)境紋理信息，并構(gòu)建四層高斯圖像金字塔，運(yùn)用FAST角點(diǎn)檢測(cè)算法提取當(dāng)前圖像中的特征信息，建立角點(diǎn)特征間的數(shù)據(jù)關(guān)聯(lián)，得到位姿估計(jì)模型；在地圖的初始繪制階段，獲取兩個(gè)關(guān)鍵幀將相機(jī)架設(shè)在移動(dòng)機(jī)器人上，通過(guò)相機(jī)與地面標(biāo)定后開(kāi)始進(jìn)行初始化過(guò)程；在初始化過(guò)程中，移動(dòng)機(jī)器人開(kāi)始移動(dòng)，同時(shí)相機(jī)捕獲當(dāng)前場(chǎng)景中角點(diǎn)信息并建立關(guān)聯(lián)，由位姿估計(jì)模型獲取當(dāng)前位姿初始估計(jì)，再運(yùn)用基于地面特征的位姿估計(jì)方法獲取當(dāng)前位姿的準(zhǔn)確估計(jì)；實(shí)現(xiàn)三維稀疏地圖的初始化后，更新關(guān)鍵幀并利用極線(xiàn)搜索與塊匹配方法建立特征點(diǎn)亞像素精度映射關(guān)系，結(jié)合位姿估計(jì)模型實(shí)現(xiàn)相機(jī)精確重定位。最后將匹配點(diǎn)投影到空間，完成當(dāng)前全局環(huán)境三維地圖創(chuàng)建。同時(shí)利用全局與局部集束調(diào)整完成算法的一致性?xún)?yōu)化。

具體包括以下步驟：

步驟1，參數(shù)校正

步驟1.1，參數(shù)定義

(1)機(jī)器人位姿

在移動(dòng)機(jī)器人起始處建立世界坐標(biāo)系，在機(jī)器人移動(dòng)過(guò)程中，其位姿可表示為：

由上式可知移動(dòng)機(jī)器人位姿可由x＝(x，y,θ)描述，其中，x，y分別表示移動(dòng)機(jī)器人在x軸，y軸方向的平移量，θ為移動(dòng)機(jī)器人繞z軸的旋轉(zhuǎn)角度。

在世界坐標(biāo)系下，相機(jī)的位姿可表示為:

其中，T_c∈SE(3)，SE(3)為特殊剛體變換群，R_c為3×3旋轉(zhuǎn)矩陣，t_c為3×1平移矩陣。該位姿矩陣建立了世界坐標(biāo)系與相機(jī)坐標(biāo)系下點(diǎn)p_c，p_w的對(duì)應(yīng)關(guān)系，即p_c＝T_cp_w。在附圖2所示，本研究中相機(jī)固定在移動(dòng)機(jī)器人上，相機(jī)與機(jī)器人間存在固定的旋轉(zhuǎn)平移關(guān)系T_cr。在機(jī)器人移動(dòng)過(guò)程中，若某一時(shí)間間隔機(jī)器人位姿增量為T(mén)_rs，與之對(duì)應(yīng)，相機(jī)的位姿增量為T(mén)_cs，則：