一種基于視覺(jué)和慣性信息的姿態(tài)與位置估計(jì)方法
【專利摘要】本發(fā)明公開(kāi)了一種基于視覺(jué)和慣性信息的姿態(tài)與位置估計(jì)方法，該方法通過(guò)融合圖像視覺(jué)和慣性信息，使用擴(kuò)展的卡爾曼濾波器保存和更新運(yùn)動(dòng)狀態(tài)信息，以計(jì)算得到設(shè)備當(dāng)前準(zhǔn)確的姿態(tài)和位置，并能獲得一段持續(xù)時(shí)間內(nèi)設(shè)備的運(yùn)動(dòng)軌跡。本發(fā)明能夠靈活地使用圖像和傳感器的信息，并通過(guò)信息的相互補(bǔ)充達(dá)到較好的魯棒性，不會(huì)出現(xiàn)追蹤丟失的情況。本發(fā)明采用的擴(kuò)展卡爾曼濾波器是基于迭代的形式計(jì)算的，與批計(jì)算的方法相比，它不需要所有的觀測(cè)量都獲得才開(kāi)始計(jì)算，同時(shí)其計(jì)算量相對(duì)較小，可以適應(yīng)終端設(shè)備計(jì)算資源較少的條件，能夠較好地達(dá)到實(shí)時(shí)性的要求。
【專利說(shuō)明】一種基于視覺(jué)和慣性信息的姿態(tài)與位置估計(jì)方法

【技術(shù)領(lǐng)域】
[0001]本發(fā)明屬于設(shè)備姿態(tài)與位置跟蹤【技術(shù)領(lǐng)域】，具體涉及一種基于視覺(jué)和慣性信息的姿態(tài)與位置估計(jì)方法。

【背景技術(shù)】
[0002]目前終端設(shè)備上常用的定位方式主要有兩種:一是利用慣性單元，即主要通過(guò)陀螺儀、重力加速度計(jì)和加速度計(jì)的數(shù)據(jù)來(lái)得到設(shè)備位置；二是通過(guò)攝像頭采集到的視覺(jué)信息來(lái)處理得到設(shè)備的姿態(tài)信息。前一種方法由于傳感器精度限制，容易產(chǎn)生誤差的累計(jì)，造成位置的估計(jì)漂移，在設(shè)備的運(yùn)動(dòng)速度較小時(shí)更為明顯，特別是零速度檢測(cè)仍然是一個(gè)難題。第二種方法基于視覺(jué)的信息，利用多視幾何的方法恢復(fù)出視覺(jué)終端相對(duì)于場(chǎng)景的位置，該類方法不存在誤差累計(jì)的問(wèn)題，但受限于視覺(jué)信息的限制，該類方法無(wú)法得到場(chǎng)景中尺度，只能作相對(duì)方位的估計(jì)，且要求終端的移動(dòng)速度應(yīng)較小，否則會(huì)由于圖像幀間匹配點(diǎn)較少產(chǎn)生較大的計(jì)算偏差甚至位置丟失等現(xiàn)象。
[0003]圖像和傳感器數(shù)據(jù)是終端設(shè)備用于計(jì)算位置和姿態(tài)信息的主要信息的來(lái)源；隨著智能設(shè)備的計(jì)算能力的提高，出現(xiàn)了一些融合圖像和傳感器數(shù)據(jù)的位置和姿態(tài)估計(jì)方法，目前主要的方法有:使用無(wú)損卡爾曼濾波(UKF，Unscented Kalman Filter)為基礎(chǔ)，其實(shí)現(xiàn)的方式是緊耦合的，只有同時(shí)獲得了圖像數(shù)據(jù)和傳感器數(shù)據(jù)時(shí)才可以用于計(jì)算當(dāng)前的位置和姿態(tài)，由于攝像頭的采樣頻率與傳感器的采樣頻率在一般情況下是不同的，這種方法一方面會(huì)由于圖像信息和慣性信息不在同一時(shí)刻采樣會(huì)引入一定的時(shí)間誤差，另一方面由于傳感器的采樣頻率較快，需要丟棄一部分?jǐn)?shù)據(jù)，造成數(shù)據(jù)的浪費(fèi)，影響最后的精度。多采樣率問(wèn)題是圖像和傳感器融合算法中要解決的重要問(wèn)題，為了使圖像和傳感器的數(shù)據(jù)同步，目前很多方法采用的是硬件解決的方法，即通過(guò)傳感器觸發(fā)攝像頭，使圖像和傳感器數(shù)據(jù)在同一時(shí)刻采樣獲得，其使用卡爾曼濾波器將傳感器的數(shù)據(jù)作為輸入，預(yù)測(cè)攝像頭的運(yùn)動(dòng)，當(dāng)獲得圖像數(shù)據(jù)時(shí)，即使用由卡爾曼濾波器預(yù)測(cè)獲得的結(jié)果用于圖像的匹配和攝像頭位置的估計(jì)。該類方法的主要優(yōu)點(diǎn)是利用慣性傳感器預(yù)測(cè)設(shè)備的運(yùn)動(dòng)，加快圖像的特征匹配速遞，但無(wú)法在最后的精確度上作很大的提高。同時(shí)為了使傳感器采樣時(shí)能夠觸發(fā)攝像頭采集圖像，也需要對(duì)硬件作出相應(yīng)的改變。


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

[0004]針對(duì)現(xiàn)有技術(shù)所存在的上述技術(shù)問(wèn)題，本發(fā)明提供了一種基于視覺(jué)和慣性信息的姿態(tài)與位置估計(jì)方法，能夠有效得融合視覺(jué)和慣性傳感器的數(shù)據(jù)信息，得到終端設(shè)備的準(zhǔn)確的位置和姿態(tài)信息。
[0005]一種基于視覺(jué)和慣性信息的姿態(tài)與位置估計(jì)方法，包括如下步驟:
[0006]首先，對(duì)安裝在設(shè)備上的攝像機(jī)和傳感器進(jìn)行聯(lián)合標(biāo)定，獲得攝像機(jī)的內(nèi)部參數(shù)和畸變參數(shù)以及攝像機(jī)和傳感器在世界坐標(biāo)系中的位置信息；所述的傳感器包括陀螺儀和加速度計(jì)；
[0007]然后，初始化EKF (擴(kuò)展卡爾曼濾波器)的狀態(tài)向量，根據(jù)上述內(nèi)部參數(shù)、畸變參數(shù)以及位置信息利用攝像機(jī)采集得到的圖像或傳感器采集得到的數(shù)據(jù)對(duì)EKF的狀態(tài)向量進(jìn)行更新，進(jìn)而從狀態(tài)向量中提取出設(shè)備的姿態(tài)與位置。
[0008]利用圖像對(duì)狀態(tài)向量進(jìn)行更新的具體過(guò)程如下:
[0009]1.1對(duì)當(dāng)前幀圖像進(jìn)行FAST特征提取，得到圖像的特征點(diǎn)；
[0010]1.2使當(dāng)前幀圖像的特征點(diǎn)與上一幀圖像的特征點(diǎn)進(jìn)行匹配，利用匹配上的特征點(diǎn)計(jì)算出兩幀圖像的本質(zhì)矩陣；進(jìn)而對(duì)所述的本質(zhì)矩陣進(jìn)行奇異值分解(SVD)得到當(dāng)前攝像機(jī)的旋轉(zhuǎn)向量和位移向量；
[0011]1.3利用EKF當(dāng)前狀態(tài)向量中的速度信息對(duì)所述的位移向量進(jìn)行校正，再利用校正后的位移向量更新EKF當(dāng)前狀態(tài)向量中的位置信息，同時(shí)利用所述的旋轉(zhuǎn)向量更新EKF當(dāng)前狀態(tài)向量中的姿態(tài)信息。
[0012]所述的步驟1.3中根據(jù)以下算式對(duì)位移向量進(jìn)行校正:
[0013]S，= (0.5+α ) λ s+(0.5_ a ) (vt+0.5at2)
[0014]其中:s和s’分別為校正前后的位移向量，α為加權(quán)因子且α = 1/(ν+2)，λ為s與S，之間的比例因子，V和a分別為EKF當(dāng)前狀態(tài)向量中的速度量和加速度量，t為上一次更新EKF狀態(tài)向量至當(dāng)前攝像機(jī)采集的時(shí)間間隔。
[0015]利用數(shù)據(jù)對(duì)狀態(tài)向量進(jìn)行更新的具體過(guò)程如下:
[0016]2.1利用陀螺儀采集到設(shè)備的當(dāng)前姿態(tài)將加速度計(jì)采集到設(shè)備的當(dāng)前加速度量投影到世界坐標(biāo)系中；
[0017]2.2根據(jù)設(shè)備的當(dāng)前加速度量以及EKF當(dāng)前狀態(tài)向量中的速度信息利用一次插值的方法加權(quán)計(jì)算得到設(shè)備的當(dāng)前速度估計(jì)量；
[0018]2.3利用設(shè)備的當(dāng)前加速度量和當(dāng)前速度估計(jì)量更新EKF當(dāng)前狀態(tài)向量中的速度信息，同時(shí)利用陀螺儀采集到設(shè)備的當(dāng)前姿態(tài)更新EKF當(dāng)前狀態(tài)向量中的姿態(tài)信息。
[0019]所述的步驟2.2中根據(jù)以下算式計(jì)算設(shè)備的當(dāng)前速度估計(jì)量:
[0020]V’ = v+0.5 (a，+a) t’
[0021]其中:v’為設(shè)備的當(dāng)前速度估計(jì)量，V和a分別為EKF當(dāng)前狀態(tài)向量中的速度量和加速度量，a’為設(shè)備的當(dāng)前加速度量，t’為上一次更新EKF狀態(tài)向量至當(dāng)前傳感器采集的時(shí)間間隔。
[0022]本發(fā)明直接使用由圖像提取的特征和經(jīng)過(guò)預(yù)處理的傳感器數(shù)據(jù)作為擴(kuò)展卡爾曼濾波器的輸入，卡爾曼濾波器的狀態(tài)包含終端設(shè)備的當(dāng)前姿態(tài)和位置信息。相比于常用的方法，本發(fā)明的不同之處在于，當(dāng)圖像的特征或傳感器的數(shù)據(jù)到達(dá)時(shí)(由于兩者的采樣頻率不同，其達(dá)到一般不會(huì)在同一時(shí)刻)，即可用于更新?tīng)顟B(tài)，而不必等圖像特征或傳感器數(shù)據(jù)一起到達(dá)，這樣可以提高狀態(tài)的更新速率，提高結(jié)果的精確度，通過(guò)多次迭代的方式避免了其他方法中使用到的全局優(yōu)化過(guò)程。
[0023]其中擴(kuò)展卡爾曼濾波器維持一個(gè)狀態(tài)向量表示當(dāng)前的狀態(tài)，它包括位置(相對(duì)于初始位置的位移)、速度和加速度信息，同時(shí)保存一個(gè)地圖信息，它包括設(shè)備的當(dāng)前旋轉(zhuǎn)向量和由圖像序列中的特征點(diǎn)構(gòu)成的場(chǎng)景地圖。當(dāng)新的圖像到達(dá)時(shí)，首先計(jì)算當(dāng)前時(shí)刻設(shè)備相對(duì)于地圖的旋轉(zhuǎn)量和平移量，再依次重新計(jì)算得到其他狀態(tài)信息，更新?tīng)顟B(tài)向量。類似的，若到達(dá)的是傳感器的數(shù)據(jù)，也同樣更新卡爾曼濾波器的狀態(tài)向量中相應(yīng)的部分。
[0024]本發(fā)明能夠靈活地使用圖像和傳感器的信息，并通過(guò)信息的相互補(bǔ)充達(dá)到較好的魯棒性，不會(huì)出現(xiàn)追蹤丟失的情況。本發(fā)明采用的擴(kuò)展卡爾曼濾波器是基于迭代的形式計(jì)算的，與批計(jì)算(如光束法平差)的方法相比，它不需要所有的觀測(cè)量都獲得才開(kāi)始計(jì)算，同時(shí)其計(jì)算量相對(duì)較小，可以適應(yīng)終端設(shè)備計(jì)算資源較少的條件，能夠較好地達(dá)到實(shí)時(shí)性的要求。

【專利附圖】

【附圖說(shuō)明】
[0025]圖1為本發(fā)明方法的流程示意圖。

【具體實(shí)施方式】
[0026]為了更為具體地描述本發(fā)明，下面結(jié)合附圖及【具體實(shí)施方式】對(duì)本發(fā)明的技術(shù)方案進(jìn)行詳細(xì)說(shuō)明。
[0027]將本發(fā)明方法在智能手機(jī)上實(shí)現(xiàn)，利用手機(jī)上的攝像頭和傳感器數(shù)據(jù)來(lái)進(jìn)行位置和姿態(tài)的計(jì)算，如圖1所示，其【具體實(shí)施方式】如下:
[0028]A.對(duì)設(shè)備上的攝像頭和傳感器進(jìn)行聯(lián)合標(biāo)定，獲得攝像機(jī)的內(nèi)部參數(shù)和畸變參數(shù)，同時(shí)獲得傳感器與攝像頭在世界坐標(biāo)系中的位置關(guān)系，使傳感器和攝像頭在世界坐標(biāo)系中對(duì)齊。
[0029]B.設(shè)置攝像頭的初始位置世界坐標(biāo)系的原點(diǎn)，在初始狀態(tài)下設(shè)置擴(kuò)展卡爾曼濾波器(EKF)的初始狀態(tài)向量，設(shè)置初始速度、位置為零，將初始位置和方向設(shè)置為世界坐標(biāo)系O
[0030]卡爾曼濾波器的狀態(tài)可由新的圖像幀或者傳感器數(shù)據(jù)的到來(lái)得到更新。圖像幀到來(lái)時(shí)的過(guò)程由C步驟實(shí)現(xiàn)，傳感器數(shù)據(jù)的到來(lái)的處理由步驟D實(shí)現(xiàn)。使用卡爾曼濾波器中狀態(tài)信息即可知道當(dāng)前的位置和姿態(tài)。保存每一次卡爾曼濾波器的狀態(tài)向量、攝像頭的旋轉(zhuǎn)量和相應(yīng)圖像的特征點(diǎn)，構(gòu)成整個(gè)場(chǎng)景的地圖。
[0031]C.為了便于在計(jì)算能力較小的設(shè)備上實(shí)現(xiàn)本實(shí)施方式，對(duì)輸入的視頻序列提取FAST特征點(diǎn)。
[0032]使用步驟B中得到的特征點(diǎn)，與上一幀作FAST特征點(diǎn)匹配，計(jì)算兩次匹配之間的本質(zhì)矩陣，使用SVD獲得相機(jī)的旋轉(zhuǎn)量和位移量，由視覺(jué)部分得到位移量只是相對(duì)尺度，應(yīng)使用卡爾曼濾波器中的位置信息對(duì)該相對(duì)位移進(jìn)行校正，校正的公式如下:
[0033]Si' = (0.5+α ) λ Si+(0.5_ a ) (vt+0.5at2)
[0034]其中:s/和&分別表示當(dāng)前時(shí)刻的真實(shí)位移和由SVD分解得到的相對(duì)位移量，λ表示相對(duì)位移與真實(shí)位移之間的比例因子，在卡爾曼濾波器狀態(tài)向量中保存并更新，V為卡爾曼濾波器中的速度量，a為卡爾曼濾波器中的加速度量，t為上一次更新卡爾曼濾波器到當(dāng)前圖像采樣的時(shí)間間隔。α為加權(quán)因子，其值在O到0.5之間，其大小通過(guò)如下表達(dá)式確定:α = I/(v+2)，隨著速度的增大，由加速度計(jì)估計(jì)的位置信息越來(lái)越準(zhǔn)確，因此其在位置估計(jì)中的權(quán)重也越來(lái)越大。
[0035]校正后作為卡爾曼濾波器的輸入，更新卡爾曼濾波器的狀態(tài)向量。
[0036]D.當(dāng)傳感器數(shù)據(jù)到達(dá)時(shí)，利用陀螺儀將加速度計(jì)的數(shù)據(jù)投影到世界坐標(biāo)系，利用一次插值的方法結(jié)合卡爾曼濾波器狀態(tài)向量中的速度分量計(jì)算新的速度估計(jì)量，一次插值的計(jì)算公式如下:
[0037]Vi= V H+0.5 (aJaH) t
[0038]將得到的速度量作為卡爾曼濾波器的輸入，使用卡爾曼濾波中狀態(tài)向量的位置信息計(jì)算新的位置信息，卡爾曼濾波器的狀態(tài)表示為:Zk= [ak, vk, sk, λ,]，用于表征設(shè)備的加速度、速度和位置信息，新的狀態(tài)由以下模型得到，zk+1=F(zk/ )，F(xiàn)使用擴(kuò)展卡爾曼濾波器的預(yù)測(cè)方法。
[0039]其中zk’利用Zk結(jié)合當(dāng)前輸入來(lái)得到，若新得到的是圖像數(shù)據(jù)，則更新校正Zk中的Sk量，若到來(lái)的是傳感器數(shù)據(jù)，則更新z k中的ak和vk。
[0040]將本實(shí)施方式應(yīng)用于智能手機(jī)等配置有攝像頭和傳感器的設(shè)備，通過(guò)將融合攝像頭的圖像信息、加速計(jì)的加速度數(shù)據(jù)和陀螺儀的方向數(shù)據(jù)可以得到手機(jī)在場(chǎng)景中實(shí)時(shí)位置和姿態(tài)。在其他配置了攝像頭和相關(guān)傳感器的設(shè)備上(如自主飛行器、機(jī)器人等)同樣可以通過(guò)本實(shí)施方式得到這些設(shè)備的位置和姿態(tài)信息，用于指導(dǎo)設(shè)備在場(chǎng)景的運(yùn)動(dòng)。
[0041]上述的對(duì)實(shí)施例的描述是為便于本【技術(shù)領(lǐng)域】的普通技術(shù)人員能理解和應(yīng)用本發(fā)明。熟悉本領(lǐng)域技術(shù)的人員顯然可以容易地對(duì)上述實(shí)施例做出各種修改，并把在此說(shuō)明的一般原理應(yīng)用到其他實(shí)施例中而不必經(jīng)過(guò)創(chuàng)造性的勞動(dòng)。因此，本發(fā)明不限于上述實(shí)施例，本領(lǐng)域技術(shù)人員根據(jù)本發(fā)明的揭示，對(duì)于本發(fā)明做出的改進(jìn)和修改都應(yīng)該在本發(fā)明的保護(hù)范圍之內(nèi)。
【權(quán)利要求】
1.一種基于視覺(jué)和慣性信息的姿態(tài)與位置估計(jì)方法，包括如下步驟: 首先，對(duì)安裝在設(shè)備上的攝像機(jī)和傳感器進(jìn)行聯(lián)合標(biāo)定，獲得攝像機(jī)的內(nèi)部參數(shù)和畸變參數(shù)以及攝像機(jī)和傳感器在世界坐標(biāo)系中的位置信息；所述的傳感器包括陀螺儀和加速度計(jì)； 然后，初始化EKF的狀態(tài)向量，根據(jù)上述內(nèi)部參數(shù)、畸變參數(shù)以及位置信息利用攝像機(jī)采集得到的圖像或傳感器采集得到的數(shù)據(jù)對(duì)EKF的狀態(tài)向量進(jìn)行更新，進(jìn)而從狀態(tài)向量中提取出設(shè)備的姿態(tài)與位置。
2.根據(jù)權(quán)利要求1所述的姿態(tài)與位置估計(jì)方法，其特征在于:利用圖像對(duì)狀態(tài)向量進(jìn)行更新的具體過(guò)程如下: 1.1對(duì)當(dāng)前幀圖像進(jìn)行FAST特征提取，得到圖像的特征點(diǎn)； 1.2使當(dāng)前幀圖像的特征點(diǎn)與上一幀圖像的特征點(diǎn)進(jìn)行匹配，利用匹配上的特征點(diǎn)計(jì)算出兩幀圖像的本質(zhì)矩陣；進(jìn)而對(duì)所述的本質(zhì)矩陣進(jìn)行奇異值分解得到當(dāng)前攝像機(jī)的旋轉(zhuǎn)向量和位移向量； 1.3利用EKF當(dāng)前狀態(tài)向量中的速度信息對(duì)所述的位移向量進(jìn)行校正，再利用校正后的位移向量更新EKF當(dāng)前狀態(tài)向量中的位置信息，同時(shí)利用所述的旋轉(zhuǎn)向量更新EKF當(dāng)前狀態(tài)向量中的姿態(tài)信息。
3.根據(jù)權(quán)利要求2所述的姿態(tài)與位置估計(jì)方法，其特征在于:所述的步驟1.3中根據(jù)以下算式對(duì)位移向量進(jìn)行校正:
S，= (0.5+ α ) λ s+ (0.5- α ) (vt+0.5at2) 其中:s和s’分別為校正前后的位移向量，α為加權(quán)因子且α = 1/(ν+2)，λ為s與s’之間的比例因子，V和a分別為EKF當(dāng)前狀態(tài)向量中的速度量和加速度量，t為上一次更新EKF狀態(tài)向量至當(dāng)前攝像機(jī)采集的時(shí)間間隔。
4.根據(jù)權(quán)利要求1所述的姿態(tài)與位置估計(jì)方法，其特征在于:利用數(shù)據(jù)對(duì)狀態(tài)向量進(jìn)行更新的具體過(guò)程如下: 2.1利用陀螺儀采集到設(shè)備的當(dāng)前姿態(tài)將加速度計(jì)采集到設(shè)備的當(dāng)前加速度量投影到世界坐標(biāo)系中； 2.2根據(jù)設(shè)備的當(dāng)前加速度量以及EKF當(dāng)前狀態(tài)向量中的速度信息利用一次插值的方法加權(quán)計(jì)算得到設(shè)備的當(dāng)前速度估計(jì)量； 2.3利用設(shè)備的當(dāng)前加速度量和當(dāng)前速度估計(jì)量更新EKF當(dāng)前狀態(tài)向量中的速度信息，同時(shí)利用陀螺儀采集到設(shè)備的當(dāng)前姿態(tài)更新EKF當(dāng)前狀態(tài)向量中的姿態(tài)信息。
5.根據(jù)權(quán)利要求4所述的姿態(tài)與位置估計(jì)方法，其特征在于:所述的步驟2.2中根據(jù)以下算式計(jì)算設(shè)備的當(dāng)前速度估計(jì)量:
V，= v+0.5 (a，+a) t' 其中為設(shè)備的當(dāng)前速度估計(jì)量，V和a分別為EKF當(dāng)前狀態(tài)向量中的速度量和加速度量，a’為設(shè)備的當(dāng)前加速度量，t’為上一次更新EKF狀態(tài)向量至當(dāng)前傳感器采集的時(shí)間間隔。
【文檔編號(hào)】G01C21/20GK104501814SQ201410765687
【公開(kāi)日】2015年4月8日 申請(qǐng)日期:2014年12月12日 優(yōu)先權(quán)日:2014年12月12日
【發(fā)明者】林城, 王梁昊, 李東曉, 張明 申請(qǐng)人:浙江大學(xué)