一種微型無(wú)人機(jī)室內(nèi)定位方法
【專利摘要】本發(fā)明涉及一種微型無(wú)人機(jī)室內(nèi)定位方法，屬于微型無(wú)人機(jī)導(dǎo)航定位【技術(shù)領(lǐng)域】；該方法包括：實(shí)時(shí)讀取無(wú)人機(jī)的當(dāng)前的三軸角速度、三軸加速度和三軸磁分量，解算采樣時(shí)刻的姿態(tài)角；實(shí)時(shí)讀取當(dāng)前幀m的RGB圖像和深度圖像，取此時(shí)刻無(wú)人機(jī)姿態(tài)角；計(jì)算當(dāng)前幀無(wú)人機(jī)和上一幀無(wú)人機(jī)的姿態(tài)差對(duì)應(yīng)的姿態(tài)矩陣；提取分布均勻的特征點(diǎn)，并計(jì)算特征向量；計(jì)算特征點(diǎn)集中每個(gè)特征點(diǎn)的距離；選出該特征點(diǎn)與最近的兩個(gè)特征點(diǎn)中的一個(gè)特征點(diǎn)，組成一個(gè)匹配對(duì)；將計(jì)算當(dāng)前幀RGB圖像的匹配對(duì)集中所有特征點(diǎn)對(duì)應(yīng)的三維位置；估計(jì)出上一幀到當(dāng)前幀期間無(wú)人機(jī)的位移；計(jì)算出的當(dāng)前幀無(wú)人機(jī)的姿態(tài)矩陣和位移。本發(fā)明減少了計(jì)算量，也一定程度上提高算法的穩(wěn)定性。
【專利說(shuō)明】一種微型無(wú)人機(jī)室內(nèi)定位方法

【技術(shù)領(lǐng)域】
[0001] 本發(fā)明屬于微型無(wú)人機(jī)導(dǎo)航定位【技術(shù)領(lǐng)域】，特別涉及一種室內(nèi)無(wú) GPS情況下MEMS 慣性傳感器輔助的RGB-D傳感器的微型無(wú)人機(jī)定位方法及系統(tǒng)。

【背景技術(shù)】
[0002] 近年來(lái)，微型無(wú)人機(jī)領(lǐng)域快速發(fā)展，尤其無(wú)人機(jī)室內(nèi)自主飛行領(lǐng)域快速前進(jìn)，出現(xiàn) 了許多突破性的進(jìn)展，將使微小型自主無(wú)人機(jī)在工業(yè)生產(chǎn)、環(huán)境探測(cè)、緊急救援甚至日常生 活中產(chǎn)生重要的作用。雖然室內(nèi)自主微小無(wú)人機(jī)具有巨大的潛力，但其發(fā)展尚面臨著巨大 挑戰(zhàn)，其中一個(gè)就是在未知室內(nèi)環(huán)境的準(zhǔn)確和高精度定位技術(shù)。由于受到自身尺寸和載荷 的嚴(yán)重限制，無(wú)人機(jī)使用的慣性傳感器只能是低性能低精度的MEMS(微機(jī)電系統(tǒng)）類型的 導(dǎo)航傳感器。該類慣性傳感器僅能提供一個(gè)精度較低的姿態(tài)信息，而不能提供一個(gè)可用的 位置和速度信息。而當(dāng)無(wú)人機(jī)在室內(nèi)作業(yè)時(shí)通常是沒(méi)有GPS(全球定位系統(tǒng)）信號(hào)的，故此 時(shí)必須通過(guò)其他傳感器來(lái)獲取一個(gè)有效的高精度的定位信息來(lái)現(xiàn)實(shí)無(wú)人機(jī)的室內(nèi)自主飛 行并執(zhí)行任務(wù)。
[0003] 目前，未知室內(nèi)環(huán)境定位方法通常采用視覺(jué)傳感器來(lái)實(shí)現(xiàn)，常見(jiàn)的視覺(jué)設(shè)備包 括激光傳感器、單目相機(jī)、雙目相機(jī)和RGB-D (彩色和深度）相機(jī)等。激光傳感器使用 ICP(Iterative Closest Point,最近鄰點(diǎn)迭代算法）對(duì)掃描點(diǎn)陣進(jìn)行配準(zhǔn)和定位，該算法 具有計(jì)算實(shí)時(shí)性好和輸出定位信息穩(wěn)定，有許多國(guó)內(nèi)外的實(shí)驗(yàn)室已經(jīng)利用該方法實(shí)現(xiàn)了某 些特定室內(nèi)環(huán)境中的定位和無(wú)人機(jī)自主飛行。該方法的主要缺點(diǎn)是只能得到的二維的掃描 信息，適用于多垂直面的環(huán)境，而在復(fù)雜的三維環(huán)境中顯得感知能力不夠。對(duì)于單目相機(jī)來(lái) 說(shuō)，一般使用SFM (Structure From Motion,運(yùn)動(dòng)結(jié)構(gòu)估計(jì)）方法來(lái)計(jì)算基本矩陣，從而得 到相機(jī)的運(yùn)動(dòng)方向，但該方法無(wú)法恢復(fù)出運(yùn)動(dòng)距離，使得該方法不能用于未知的復(fù)雜室內(nèi) 環(huán)境。與單目相機(jī)相比，雙目視覺(jué)系統(tǒng)能夠恢復(fù)出圖中許多點(diǎn)的深度，將圖片中的像素點(diǎn)映 射到三維空間中，從而獲得三維的深度信息，再利用這些三維深度信息的關(guān)聯(lián)關(guān)系解算出 相機(jī)系統(tǒng)的運(yùn)動(dòng)方向和距離。該方法的一個(gè)缺點(diǎn)是對(duì)于相機(jī)的標(biāo)定很嚴(yán)格，價(jià)格昂貴。而 RGB-D相機(jī)得到環(huán)境和雙目相機(jī)類似，能直接獲取空間點(diǎn)到相機(jī)的三維位置信息和普通的 二維圖像信息，通過(guò)和雙目相機(jī)類似的方法，同樣可獲取相機(jī)的運(yùn)動(dòng)方向和距離這6個(gè)自 由度的信息。相比雙目相機(jī)，RGB-D相機(jī)優(yōu)勢(shì)是價(jià)格低廉，但是其數(shù)據(jù)質(zhì)量比較差，噪聲大， 同時(shí)存在固有的數(shù)據(jù)延遲等問(wèn)題。
[0004] 基于RGB-D相機(jī)的室內(nèi)定位算法通常是提取RGB圖像的特征點(diǎn)，并將相鄰兩幀 的RGB圖像的特征點(diǎn)進(jìn)行匹配，再利用匹配的特征點(diǎn)所對(duì)應(yīng)的景深信息來(lái)估計(jì)這兩幀之 間相機(jī)的6維運(yùn)動(dòng)情況，即姿態(tài)變化和平移運(yùn)動(dòng)。這個(gè)過(guò)程被稱為視覺(jué)位置估計(jì)（Visual Odometry)或簡(jiǎn)稱V0。這個(gè)方法一般存在如下三個(gè)問(wèn)題：一是圖像特征提取算法耗時(shí)巨大， 導(dǎo)致該算法不能實(shí)時(shí)運(yùn)行；二是圖像特征提取由于環(huán)境而集中在某個(gè)局部，導(dǎo)致定位算法 性能下降，甚至估計(jì)錯(cuò)誤；三是利用關(guān)聯(lián)的三維點(diǎn)對(duì)直接估計(jì)出六維的運(yùn)動(dòng)，容易受到噪聲 和錯(cuò)誤數(shù)據(jù)的影響，而導(dǎo)致姿態(tài)變化和平移運(yùn)動(dòng)相互影響，進(jìn)而產(chǎn)生更大的估計(jì)誤差。


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

[0005] 本發(fā)明的目的是針對(duì)已有技術(shù)存在的問(wèn)題，提供一種微型無(wú)人機(jī)室內(nèi)定位方法， 以實(shí)現(xiàn)微小無(wú)人機(jī)的實(shí)時(shí)、魯棒、高精度的室內(nèi)位置估計(jì)。
[0006] 為實(shí)現(xiàn)上述目的，本發(fā)明提供了一種微型無(wú)人機(jī)室內(nèi)定位方法，其特征在于，該無(wú) 人機(jī)載有MEMS慣性器件及RGB-D相機(jī)，其中MEMS慣性器件以采樣間隔Ts輸出無(wú)人機(jī)的當(dāng) 前的三軸角速度

【權(quán)利要求】
1· 一種微型無(wú)人機(jī)室內(nèi)定位方法，其特征在于，該無(wú)人機(jī)載有MEMS慣性器件及RGB-D 相機(jī)，其中MEMS慣性器件以采樣間隔Ts輸出無(wú)人機(jī)的當(dāng)前的三軸角速度
三 軸加速度
和無(wú)人機(jī)所在位置的地球磁場(chǎng)的三軸磁分量
上角標(biāo) b表示該數(shù)據(jù)是測(cè)量值在機(jī)體坐標(biāo)系b中的表示結(jié)果，RGB-D相機(jī)以30Hz輸出RGB圖像和 深度圖像，該方法包括以下步驟： 1) 實(shí)時(shí)讀取MEMS慣性器件的無(wú)人機(jī)的當(dāng)前的三軸角速度
v三軸加速度
和無(wú)人機(jī)所在位置的地球磁場(chǎng)的三軸磁分量
，解算無(wú)人機(jī)的當(dāng) 前米樣時(shí)刻η的姿態(tài)角
，其中
分別表示俯仰角、橫滾角和偏航角； 2) 實(shí)時(shí)讀取RGB-D相機(jī)的當(dāng)前幀m的RGB圖像和深度圖像，取此時(shí)刻步驟1)中無(wú)人機(jī) 姿態(tài)角
作為當(dāng)前幀RGB圖像的無(wú)人機(jī)姿態(tài)角（
；設(shè)當(dāng)前幀m和上一 幀m-Ι的無(wú)人機(jī)姿態(tài)角對(duì)應(yīng)的姿態(tài)矩陣分別為心和R^，則當(dāng)前幀mRGB圖像的無(wú)人機(jī)姿態(tài) 和上一幀m-1RGB圖像的無(wú)人機(jī)姿態(tài)的姿態(tài)差對(duì)應(yīng)的姿態(tài)矩陣δ Rm ; 3) 從當(dāng)前幀RGB圖像中提取一定數(shù)量且分布均勻的特征點(diǎn)，并計(jì)算特征點(diǎn)對(duì)應(yīng)的特征 向量； 4) 計(jì)算當(dāng)前幀RGB圖像的特征點(diǎn)集中每個(gè)特征點(diǎn)與前一幀RGB圖像的特征點(diǎn)集中每個(gè) 特征點(diǎn)的距離；對(duì)當(dāng)前幀RGB圖像的特征點(diǎn)集中的每個(gè)特征點(diǎn)選出該特征點(diǎn)與前一幀RGB 圖像的特征點(diǎn)集中與該特征點(diǎn)最近的兩個(gè)特征點(diǎn)中的一個(gè)特征點(diǎn)，組成當(dāng)前幀RGB圖像的 一個(gè)匹配對(duì)；將當(dāng)前幀RGB圖像的所有匹配對(duì)組成當(dāng)前幀RGB圖像的匹配對(duì)集C m ; 5) 計(jì)算當(dāng)前幀RGB圖像的匹配對(duì)集Cm中所有特征點(diǎn)對(duì)應(yīng)的三維位置； 6) 結(jié)合姿態(tài)矩陣δ &和當(dāng)前幀三維點(diǎn)匹配對(duì)集合之，使用RANSAC方法估計(jì)出上一幀 到當(dāng)前幀之間無(wú)人機(jī)的位移L; 7) 根據(jù)上一幀到當(dāng)前幀之間無(wú)人機(jī)的位移^計(jì)算出的當(dāng)前幀無(wú)人機(jī)的姿態(tài)矩陣為Rm、 位移Χπ;即可確定無(wú)人機(jī)相對(duì)于起飛點(diǎn)的姿態(tài)和位置信息，即完成了無(wú)人機(jī)的室內(nèi)定位功 能。
2.如權(quán)利要求1所述方法，其特征在于，所述步驟1)具體包括以下步驟： 11) 實(shí)時(shí)讀取MEMS慣性器件的三軸角速度數(shù)據(jù)，設(shè)上一采樣時(shí)刻η-1通過(guò)三軸角速度 計(jì)算的相機(jī)姿態(tài)的四元數(shù)為qn-i，用四元數(shù)法計(jì)算當(dāng)前采樣時(shí)刻η的通過(guò)三軸角速度計(jì)算 的相機(jī)姿態(tài)四元數(shù)q n如下式：
然后將qn轉(zhuǎn)換（公知方法）成對(duì)應(yīng)的姿態(tài)角（
12) 設(shè)三軸加速度判斷式如下：
其中g(shù)Q是當(dāng)?shù)刂亓铀俣戎担? 實(shí)時(shí)讀取MEMS慣性器件的三軸加速度和三軸磁分量，若讀取的三軸加速度不滿足判 斷式，則當(dāng)前采樣時(shí)刻η無(wú)人機(jī)的姿態(tài)角（θη，Υη，ψη)如下式：
， 并轉(zhuǎn)到步驟2); 若讀取的三軸加速度滿足判斷式，則轉(zhuǎn)到步驟13); 13)求解式下式可得當(dāng)前采樣時(shí)刻η三軸加速度和三軸磁分量計(jì)算的另一種姿態(tài)角
，其對(duì)應(yīng)的姿態(tài)矩陣為瓦；
其中和是當(dāng)?shù)氐卮诺膟和ζ方向的磁分量，上標(biāo)g表示該數(shù)據(jù)是測(cè)量值在當(dāng)?shù)? 地理坐標(biāo)系g中的表示結(jié)果； 則當(dāng)前采樣時(shí)刻η無(wú)人機(jī)的姿態(tài)角
如下式：
3.如權(quán)利要求2所述方法，其特征在于，所述步驟3)具體包括以下步驟： 31) 將當(dāng)前幀RGB圖像劃分成若干尺寸一樣的子圖像； 32) 使用ORB算法對(duì)每個(gè)子圖像進(jìn)行特征點(diǎn)提取，使得在每個(gè)子圖像上提取的的特征 點(diǎn)數(shù)量一致；將所有子圖像中提取的特征點(diǎn)組成一個(gè)當(dāng)前幀RGB圖像的特征點(diǎn)集合； 33) 使用ORB算法計(jì)算當(dāng)前幀RGB圖像的特征點(diǎn)集中每個(gè)特征點(diǎn)對(duì)應(yīng)的特征向量。
【文檔編號(hào)】G01C21/00GK104154910SQ201410350665
【公開日】2014年11月19日 申請(qǐng)日期:2014年7月22日 優(yōu)先權(quán)日:2014年7月22日
【發(fā)明者】程農(nóng), 李清, 唐良文, 吳沁凡 申請(qǐng)人:清華大學(xué)