一種基于攝像頭與重力感應(yīng)器融合的智能移動(dòng)設(shè)備六自由度位姿估計(jì)方法
【專利摘要】本發(fā)明所要解決的技術(shù)問題是提供一種基于攝像頭與重力感應(yīng)器融合的智能移動(dòng)設(shè)備六自由度位姿估計(jì)方法，包括以下步驟：標(biāo)定攝像頭的內(nèi)參數(shù)、初始化位姿估計(jì)、從攝像頭中讀取當(dāng)前幀圖像同時(shí)從重力感應(yīng)器中讀取信號(hào)、由單目視覺運(yùn)動(dòng)估計(jì)算法得到當(dāng)前幀圖像與前一幀圖像之間攝像頭的運(yùn)動(dòng)參數(shù)、用在中的姿態(tài)即傾斜角和和滾動(dòng)角構(gòu)成卡爾曼濾波器的狀態(tài)矢量、從濾波估計(jì)得到的中解算出當(dāng)前幀圖像時(shí)刻在中的姿態(tài)、然后得到的位姿。此融合位姿估計(jì)方法簡單，可以在資源有限的移動(dòng)設(shè)備上應(yīng)用。此方法僅依賴智能移動(dòng)設(shè)備普遍配置的單個(gè)攝像頭與重力感應(yīng)器，可以基于此位姿估計(jì)方法開發(fā)出多種新型的移動(dòng)設(shè)備應(yīng)用，如增強(qiáng)現(xiàn)實(shí)、局部地圖導(dǎo)航、3D游戲等。
【專利說明】一種基于攝像頭與重力感應(yīng)器融合的智能移動(dòng)設(shè)備六自由度位姿估計(jì)方法
【技術(shù)領(lǐng)域】
[0001]本發(fā)明涉及智能移動(dòng)設(shè)備的位姿估計(jì)方法，尤其涉及通過融合攝像頭視覺位姿估計(jì)技術(shù)與慣導(dǎo)位姿估計(jì)技術(shù)實(shí)現(xiàn)智能移動(dòng)設(shè)備位姿估計(jì)的方法。
【背景技術(shù)】
[0002]攝像頭與重力感應(yīng)器融合的靈感源于攝像頭慣導(dǎo)融合位姿估計(jì)。攝像頭慣導(dǎo)融合位姿估計(jì)是一個(gè)很前沿的科研方向，但是它的方法一般比較復(fù)雜，而且基本都是為車載平臺(tái)或移動(dòng)機(jī)器人設(shè)計(jì)的，所使用的慣導(dǎo)設(shè)備的功能和價(jià)格也遠(yuǎn)非重力感應(yīng)器能比。
[0003]攝像頭與重力感應(yīng)器融合位姿估計(jì)方法是專為智能移動(dòng)設(shè)備設(shè)計(jì)的，這里的智能移動(dòng)設(shè)備主要指智能手機(jī)。此融合位姿估計(jì)方法簡單，可以在資源有限的移動(dòng)設(shè)備上應(yīng)用。此方法僅依賴智能移動(dòng)設(shè)備普遍配置的單個(gè)攝像頭與重力感應(yīng)器，可以基于此位姿估計(jì)方法開發(fā)出多種新型的移動(dòng)設(shè)備應(yīng)用，如增強(qiáng)現(xiàn)實(shí)、局部地圖導(dǎo)航、3D游戲等。
【發(fā)明內(nèi)容】

[0004]本發(fā)明所要解決的技術(shù)問題是提供一種基于攝像頭與重力感應(yīng)器融合的智能移動(dòng)設(shè)備六自由度位姿估計(jì)方法，主要解決智能移動(dòng)設(shè)備的位姿估計(jì)問題。為此，本發(fā)明采用以下技術(shù)方案:
一種基于攝像頭與重力感應(yīng)器融合的智能移動(dòng)設(shè)備六自由度位姿估計(jì)方法，其特征在于，所述方法基于智能移動(dòng)設(shè)備的攝像頭與重力感應(yīng)器，所述方法包括以下步驟:
步驟一、標(biāo)定攝像頭的內(nèi)參數(shù)；標(biāo)定重力感應(yīng)器坐標(biāo)系丨相對(duì)于攝像頭坐標(biāo)系(C]的相對(duì)旋轉(zhuǎn)矩陣<。
[0005]{/丨是固聯(lián)于重力感應(yīng)器上并隨重力感應(yīng)器一起運(yùn)動(dòng)的三維坐標(biāo)系；{C丨是固聯(lián)于攝像頭上并隨攝像頭一起運(yùn)動(dòng)的二維坐標(biāo)系；重力感應(yīng)器和攝像頭都固定在智能移動(dòng)設(shè)備上之后，(/)與(C)之間的相對(duì)旋轉(zhuǎn)和相對(duì)平移是固定的，它們之間的相對(duì)旋轉(zhuǎn)用相對(duì)旋轉(zhuǎn)
矩陣< 描述。
[0006]步驟二、初始化位姿估計(jì)，包括:確定全局坐標(biāo)系(G};從初始時(shí)刻智能移動(dòng)設(shè)備的攝像頭中讀取圖像并讀取重力感應(yīng)器信息；得到〖C]在(6)中的初始位姿。[0007]步驟三、從攝像頭中讀取當(dāng)前幀圖像，同時(shí)從重力感應(yīng)器中讀取信號(hào)；由單目視覺運(yùn)動(dòng)估計(jì)算法得到當(dāng)前幀圖像與前一幀圖像之間攝像頭的運(yùn)動(dòng)參數(shù)，運(yùn)動(dòng)參數(shù)包括前一幀
圖像時(shí)刻的〖C；}相對(duì)當(dāng)前幀圖像時(shí)刻{C丨的相對(duì)平移t和相對(duì)旋轉(zhuǎn)Afl，然后結(jié)合前一幀圖像時(shí)刻【Cl在〖G)中的位姿和當(dāng)前計(jì)算的運(yùn)動(dòng)參數(shù)得到當(dāng)前幀圖像時(shí)刻(C}在丨中的位姿；從獲取的重力感應(yīng)器信號(hào)得到重力加速度在{/]中的矢量表示Λ ,再用&得到重力加速度在CC}中的矢量表示= Ri..§r ο
[0008]步驟四、用(Cl在(6)中的姿態(tài)即傾斜角?和滾動(dòng)角'3^+^:構(gòu)成卡爾曼濾波器
【權(quán)利要求】
1.一種基于攝像頭與重力感應(yīng)器融合的智能移動(dòng)設(shè)備六自由度位姿估計(jì)方法，其特征在于，所述方法基于智能移動(dòng)設(shè)備的攝像頭與重力感應(yīng)器，包括如下步驟: 步驟一、標(biāo)定攝像頭的內(nèi)參數(shù)；標(biāo)定重力感應(yīng)器坐標(biāo)系相對(duì)于攝像頭坐標(biāo)系(C]的相對(duì)旋轉(zhuǎn)矩陣<; 【/}是固聯(lián)于重力感應(yīng)器上并隨重力感應(yīng)器一起運(yùn)動(dòng)的三維坐標(biāo)系；[C]是固聯(lián)于攝像頭上并隨攝像頭一起運(yùn)動(dòng)的二維坐標(biāo)系；重力感應(yīng)器和攝像頭都固定在智能移動(dòng)設(shè)備上之后,{/}與{c}之間的相對(duì)旋轉(zhuǎn)和相對(duì)平移是固定的，CO與{c}之間的相對(duì)旋轉(zhuǎn)用相對(duì)旋轉(zhuǎn)矩Wr1c描述； 步驟二、初始化位姿估計(jì)，包括:確定全局坐標(biāo)系iG];從初始時(shí)刻智能移動(dòng)設(shè)備的攝像頭中讀取圖像并讀取重力感應(yīng)器信息；得到{C}在(G)中的初始位姿； 步驟三、從攝像頭中讀取當(dāng)前幀圖像，同時(shí)從重力感應(yīng)器中讀取信號(hào)；由單目視覺運(yùn)動(dòng)估計(jì)算法得到當(dāng)前幀圖像與前一幀圖像之間攝像頭的運(yùn)動(dòng)參數(shù)，運(yùn)動(dòng)參數(shù)包括前一幀圖像時(shí)刻的O:丨相對(duì)當(dāng)前幀圖像時(shí)刻{?]的相對(duì)平移《和相對(duì)旋轉(zhuǎn)M ,然后結(jié)合前一幀圖像時(shí)刻{C]在(G)中的位姿和當(dāng)前計(jì)算的運(yùn)動(dòng)參數(shù)得到當(dāng)前幀圖像時(shí)刻(C】在{G]中的位姿；從獲取的重力感應(yīng)器信號(hào)得到重力加速度在(I丨中的矢量表示&，再用得到重力加速度在(Cl中的矢量表示I,其中，βe =祀，氣.； 步驟四、用【C}在{G}中的姿態(tài)即傾斜角和和滾動(dòng)角構(gòu)成卡爾曼濾波器的狀態(tài)矢量
2.根據(jù)權(quán)利要求1所述的基于攝像頭與重力感應(yīng)器融合的智能移動(dòng)設(shè)備六自由度位姿估計(jì)方法，其特征在于，在所述步驟二中，采用包括以下步驟: (I )、確定全局坐標(biāo)系(G丨，(G丨的坐標(biāo)原點(diǎn)取在初始化時(shí)攝像頭坐標(biāo)系{C丨的原點(diǎn)處，Ce}的z軸正方向取為與重力方向相反,佴丨的r軸取為初始化時(shí)0丨的r軸在水平面上的投影，剩下懷丨的y軸由右手法則確定確定后就一直固定不變，智能移動(dòng)設(shè)備的六自由度位姿估計(jì)都是估計(jì)攝像頭坐標(biāo)系{C】在(G]坐標(biāo)系下的位姿，位姿包括位置姿態(tài)即傾斜角pitch和滾動(dòng)角roll,以及方位即偏擺角和妒，其中yaw、fdtch'roll是姿態(tài)方位的歐拉角表示法； (2)、從初始時(shí)刻智能移動(dòng)設(shè)備的攝像頭中讀取圖像并讀取重力感應(yīng)器信息；


(3)、得到O:)在(G丨中的初始位姿:由(G}的取法得到忙}在中的初始位置為
3.根據(jù)權(quán)利要求1所述的基于攝像頭與重力感應(yīng)器融合的智能移動(dòng)設(shè)備六自由度位姿估計(jì)方法，其特征在于，在所述步驟四中，采用包括以下步驟:(I )、設(shè){C】在(G)中的姿態(tài)方位對(duì)應(yīng)的旋轉(zhuǎn)矩陣用表示，第幀圖像時(shí)刻的{C】相對(duì)于第左幀圖像時(shí)刻(Cj的相對(duì)旋轉(zhuǎn)用表示，則有
4.根據(jù)權(quán)利要求1所述的基于攝像頭與重力感應(yīng)器融合的智能移動(dòng)設(shè)備六自由度位姿估計(jì)方法，其特征在于，在所述步驟五中，采用包括以下步驟: (I)、結(jié)合步驟四中的卡爾曼濾波器預(yù)測(cè)和觀測(cè)，并對(duì)卡爾曼濾波器的過程噪聲和觀測(cè)噪聲進(jìn)行估計(jì)，然后根據(jù)經(jīng)典卡爾曼濾波算法進(jìn)行卡爾曼濾波估計(jì)；
【文檔編號(hào)】G01B11/26GK103900473SQ201410124679
【公開日】2014年7月2日 申請(qǐng)日期:2014年3月31日 優(yōu)先權(quán)日:2014年3月31日
【發(fā)明者】黃鴻, 劉勇 申請(qǐng)人:浙江大學(xué)