本發(fā)明屬于視覺導(dǎo)航
技術(shù)領(lǐng)域：
，具體涉及一種基于多景深信息的視覺里程計(jì)測速方法。
背景技術(shù)：
：目前，車輛、自主機(jī)器人等智能化地面交通工具對導(dǎo)航需求越來越高，而在城市、山谷等區(qū)域，衛(wèi)星定位精度較差，甚至定位失效，因此在衛(wèi)星定位不可用的環(huán)境下，自主導(dǎo)航系統(tǒng)精度是需要重點(diǎn)考慮的問題之一。視覺導(dǎo)航系統(tǒng)作為新興的自主導(dǎo)航系統(tǒng)在近年來成為各國學(xué)者的研究熱點(diǎn)。視覺里程計(jì)作為視覺導(dǎo)航中的重要組成部分，已應(yīng)用于實(shí)際工程產(chǎn)品中。但是由于受圖像匹配算法復(fù)雜度與特征點(diǎn)誤匹配的存在，視覺里程計(jì)的精度仍然較低，難用于需要高精度長航時(shí)導(dǎo)航的智能化交通工具中。為解決視覺里程計(jì)面臨的兩大難題：圖像匹配算法計(jì)算復(fù)雜度與特征點(diǎn)誤匹配?？煽紤]從以下兩方面解決途徑：第一，目前常用的圖像匹配方法包括SIFT與SURF等復(fù)雜匹配算法，雖然算法的匹配度較高，但是由于為了保障匹配算法的強(qiáng)魯棒性，因此犧牲了算法的實(shí)時(shí)性，于是此類算法復(fù)雜度較高，往往只能應(yīng)用于對實(shí)時(shí)性要求不高的使用環(huán)境。因此為了降低圖像匹配復(fù)雜度，可通過引入外界參考信息，縮小圖像匹配的搜索空間，以實(shí)現(xiàn)縮短圖像匹配算法計(jì)算時(shí)間的目的。而MEMS慣導(dǎo)可提供速度與航姿信息，可作為外界輔助導(dǎo)航信息源；第二，對于二維平移運(yùn)動，利用單一特征點(diǎn)匹配實(shí)現(xiàn)對速度的測量往往會由于誤匹配導(dǎo)致速度計(jì)算誤差較大，因此，考慮引入不同景深的特征點(diǎn)分別進(jìn)行圖像匹配與運(yùn)動估計(jì)。通過綜合分析各特征點(diǎn)的 運(yùn)動估計(jì)結(jié)果，利用小波分析方法，剔除運(yùn)動估計(jì)野值特征點(diǎn)，降低測速噪聲，實(shí)現(xiàn)對視覺里程計(jì)測速精度的提高。技術(shù)實(shí)現(xiàn)要素：本發(fā)明需要解決的技術(shù)問題為：現(xiàn)有的視覺里程計(jì)測速方法由于受到圖像匹配算法復(fù)雜度與特征點(diǎn)誤匹配的存在，因此精度較低，難用于需要高精度長航時(shí)導(dǎo)航的智能化交通工具中。本發(fā)明的基于多景深信息的視覺里程計(jì)測速方法，包括以下步驟：步驟1、選擇相機(jī)：根據(jù)任務(wù)特征選擇合適的雙目相機(jī)；步驟2、誤差標(biāo)定：標(biāo)定MEMS慣導(dǎo)系統(tǒng)/視覺里程計(jì)安裝誤差；步驟3、特征點(diǎn)集提取：基于灰度方差信息提取圖像特征點(diǎn)；步驟4、特征點(diǎn)集測距：使用雙目相機(jī)實(shí)現(xiàn)特征點(diǎn)測距；步驟5、速度計(jì)算：對前后幀圖像中特征點(diǎn)進(jìn)行識別實(shí)現(xiàn)測速；步驟6、噪聲消除：運(yùn)用小波分析方法消除測速噪聲。優(yōu)選的，步驟1中所述根據(jù)任務(wù)特征選擇合適的雙目相機(jī)的具體方法為：根據(jù)運(yùn)動速度、高度、測量精度、計(jì)算機(jī)運(yùn)算速度以及其他條件選擇雙目相機(jī)的分辨率、視場角及其它相機(jī)參數(shù)。優(yōu)選的，步驟2中所述標(biāo)定MEMS慣導(dǎo)系統(tǒng)/視覺里程計(jì)安裝誤差的具體操作為：利用卡爾曼濾波，根據(jù)不同傳感器的輸出特性的差異性，實(shí)現(xiàn)對MEMS慣導(dǎo)系統(tǒng)和視覺里程計(jì)之間安裝誤差角和安裝桿臂的估計(jì)與補(bǔ)償，實(shí)現(xiàn)傳感器輸出導(dǎo)航信息的統(tǒng)一化。優(yōu)選的，步驟3中所述基于灰度方差信息提取圖像特征點(diǎn)的具體操作為：在t1時(shí)刻雙目相機(jī)中A相機(jī)采集的圖像中，將圖像分為k個(gè)子區(qū)域。設(shè)每個(gè)子區(qū)域內(nèi)有n個(gè)點(diǎn)集Q1，Q2，…Qn，任一點(diǎn)集的灰度方差為Gi(i＝1,2,…,n)，而點(diǎn)集Qi(i＝1,2,…,n)中包含m個(gè)點(diǎn)qi(j＝1,2,…,m)，對應(yīng)的灰度值為gj(j＝1,2,…,m)，那么點(diǎn)集Qi的灰度方差Gi為Gi=Σj=0m(gj-g‾i)2/mg‾i=Σj=1mgj/m]]>其中，為點(diǎn)集Qi中m個(gè)點(diǎn)的灰度均值；選取子區(qū)域內(nèi)灰度方差最大的點(diǎn)集中心點(diǎn)作為圖像特征點(diǎn)，則在t1時(shí)刻采集圖像中存在k個(gè)特征點(diǎn)集Pl(l＝1,2,…,k)。優(yōu)選的，步驟4中所述使用雙目相機(jī)實(shí)現(xiàn)特征點(diǎn)測距的具體方法為：在t1時(shí)刻雙目相機(jī)中B相機(jī)采集的圖像中識別A相機(jī)中的特征點(diǎn)集，基于空間上存在偏移的雙目圖像，完成特征點(diǎn)集所代表的實(shí)際物體到相機(jī)的距離測量；設(shè)Q是待測距的實(shí)際物體，其到相機(jī)的垂直距離為R，在A、B相機(jī)上形成的像點(diǎn)集分別是QA和QB，b是雙目相機(jī)的基線長，f是焦距，(x1-x2)為視差，其中x1為QA中心點(diǎn)的水平像素值，x2為QB中心點(diǎn)的水平像素值；利用相似三角形性質(zhì)，可得Q到相機(jī)的垂直距離R，即QA的景深為：R=b×fx1-x2]]>由上式，能夠計(jì)算出每個(gè)特征點(diǎn)集Pl的景深Rl(l＝1,2,…,k)。優(yōu)選的，步驟5中所述對前后幀圖像中特征點(diǎn)集進(jìn)行識別實(shí)現(xiàn)測速的具體方法為：在t2時(shí)刻雙目相機(jī)中A相機(jī)采集的圖像中，利用MEMS慣導(dǎo)系統(tǒng)提供的速度信息，設(shè)定t1時(shí)刻雙目相機(jī)中A相機(jī)特征點(diǎn)集Pl在t2時(shí)刻的搜索空間，利用圖像匹配算法，計(jì)算特征點(diǎn)集Pl在t1和t2時(shí)刻圖像中的像素差，并結(jié)合特征點(diǎn)集景深Rl獲得特征點(diǎn)集的運(yùn)動距離，最后根據(jù)運(yùn)動距離和時(shí)間差，計(jì)算k個(gè)不同景深特征點(diǎn)集的實(shí)時(shí)速度vl(l＝1,2,…,k)。優(yōu)選的，步驟6中所述運(yùn)用小波分析方法消除測速噪聲的具體方法為：將vl設(shè)定為原始一維信號，將一維信號做二級小波變化，設(shè)定二級小波系數(shù)w2＝[wa2wd2wd1]式中，wa2為二級近似系數(shù)，wd2為二級細(xì)節(jié)系數(shù)，wd1為一級細(xì)節(jié)系數(shù)；對原始一維信號進(jìn)行小波正變換，獲得二級近似分解a2、二級細(xì)節(jié)分解d2和一級細(xì)節(jié)分解d1；設(shè)定兩個(gè)閾值λ1和λ2，認(rèn)為一級細(xì)節(jié)分解d1中小于λ1的部分為一級噪聲，而二級細(xì)節(jié)分解d2中小于λ2的部分為二級噪聲，噪聲去除之后獲得一級小波細(xì)節(jié)d1`和二級小波細(xì)節(jié)d2`，使用下式對速度信號進(jìn)行恢復(fù)：v‾=a2+d1′+d2′]]>式中，為去除噪聲的速度信號，即視覺里程計(jì)的測速輸出。本發(fā)明的有益效果為：(1)本發(fā)明的方法在視覺里程計(jì)測速計(jì)算中加入MEMS慣導(dǎo)輔助速度信息可顯著提高測速精度，降低了計(jì)算的復(fù)雜度，適用于長航時(shí)導(dǎo)航；(2)采用多景深特征點(diǎn)來測量速度，并加入小波分析方法來剔除計(jì)算噪聲，可增強(qiáng)本方法的魯棒性，進(jìn)一步提高導(dǎo)航測速精度。附圖說明圖1為雙目測距原理示意圖；圖2為視覺里程計(jì)自定位原理示意圖；圖3視覺里程計(jì)與GPS測速對比圖。具體實(shí)施方式本發(fā)明的方法，首先利用雙目相機(jī)計(jì)算不同景深的特征點(diǎn)與相機(jī)的距離；然后，引入外界參考信息，即MEMS慣導(dǎo)的姿態(tài)與速度信息，縮小圖像匹配的搜索空間，以實(shí)現(xiàn)縮短圖像匹配算法計(jì)算時(shí)間的目的；之后，在完成前后幀圖像中各特征點(diǎn)圖像匹配后，分別估計(jì)各特征點(diǎn)的運(yùn)動距離；最后，利用小波分析方法，剔除運(yùn)動距離計(jì)算野值，減小運(yùn)動估計(jì)噪聲，實(shí)現(xiàn)對視覺里程計(jì)精度的提高。1、雙目測量景深雙目測量景深方法包括兩部分內(nèi)容：特征點(diǎn)提取與特征點(diǎn)測距。特征點(diǎn)的定義很多，包括SIFT特征點(diǎn)、Harris特征點(diǎn)等。本實(shí)施例從工程應(yīng)用角度出發(fā)，利用灰度圖像的特點(diǎn)，選取區(qū)域內(nèi)灰度變化最大的像素點(diǎn)集的中心點(diǎn)作為特征點(diǎn)。具體算法如下：設(shè)區(qū)域內(nèi)有n個(gè)點(diǎn)集Q1，Q2，…Qn，任一點(diǎn)集的灰度方差為Gi(i＝1,2,…,n)，而點(diǎn)集Qi(i＝1,2,…,n)中包含m個(gè)點(diǎn)qi(j＝1,2,…,m)，對應(yīng)的灰度值為gj(j＝1,2,…,m)，那么點(diǎn)集Qi的灰度方差Gi為Gi=Σj=0m(gj-g‾i)2/mg‾i=Σj=1mgj/m]]>特征點(diǎn)測距是基于空間上存在偏移的雙目圖像，完成其中匹配物體(左、右圖像中均存在)的距離測量，測距原理見圖1。圖中，Q是待測距的特征點(diǎn)，其到相機(jī)的垂直距離為R，在左右相機(jī)上形成的像點(diǎn)分別是Q1和Q2。b是雙目相機(jī)的基線長，f是焦距，(x1-x2)是Q點(diǎn)在左右兩幅圖像上像點(diǎn)的位置差，又稱為視差。利用相似三角形性質(zhì)，可得Q點(diǎn)到相機(jī)的垂直距離R，即Q點(diǎn)的景深為：R=b×fx1-x2]]>利用上述方法的測距精度可達(dá)1％。2、直線運(yùn)動估計(jì)立體視覺里程計(jì)的基本原理：載體t1時(shí)刻位于點(diǎn)O1，t2時(shí)刻運(yùn)動到O2。在全局世界坐標(biāo)系中，載體位置和姿態(tài)的變化可以由六個(gè)參數(shù)(Δx,Δy,Δz,Δα,Δβ,Δγ)表示，前三個(gè)參數(shù)代表位置的變化，后三個(gè)參數(shù)代表姿態(tài)的變化。如果載體在移動過程中每一幀都能確定其與前一幀時(shí)的位置 和姿態(tài)，那么就自然實(shí)現(xiàn)了載體的定位。對全局坐標(biāo)系中一點(diǎn)A，載體在t1時(shí)刻觀察到的A的坐標(biāo)為(x1,y1,z1)T，t2時(shí)刻觀察到的坐標(biāo)為(x2,y2,z2)T，假如載體姿態(tài)發(fā)生了變化，也就是代表姿態(tài)的航向角、滾轉(zhuǎn)角和俯仰角發(fā)生了變化。此三個(gè)角度的變化(Δα,Δβ,Δγ)T產(chǎn)生了3×3旋轉(zhuǎn)矩陣R，位置變化(Δx,Δy,Δz)T產(chǎn)生了平移向量T，則有：x2y2z2=Rx1y1z1+T---(1)]]>其中，R=r11r12r13r21r22r23r31r32r33,T=ΔxΔyΔz.]]>如果能在相鄰兩幀中實(shí)現(xiàn)像素點(diǎn)的跟蹤，即對前一幀圖像中的像素點(diǎn)，在下一幀圖像中能找到它的對應(yīng)點(diǎn)，并利用立體視覺分別計(jì)算出此像素點(diǎn)在前后兩幀中的三維坐標(biāo)，則根據(jù)式(1)，一對對應(yīng)的特征點(diǎn)就能產(chǎn)生三個(gè)方程。這樣，只要存在4個(gè)對應(yīng)的特征點(diǎn)，就能計(jì)算出旋轉(zhuǎn)矩陣R和平移矩陣T。如果特征點(diǎn)的數(shù)目大于4個(gè)，則需要通過多個(gè)方程求得(R,T)的最優(yōu)值。立體視覺里程計(jì)的計(jì)算復(fù)雜，尤其是求解旋轉(zhuǎn)矩陣R的難度較大，另外運(yùn)載交通工具的運(yùn)動大部分為直線運(yùn)動，且部分旋轉(zhuǎn)運(yùn)動由于旋轉(zhuǎn)角度較小，可近似擬合為多時(shí)刻直線運(yùn)動。因此本方法著重考慮的是提高直線運(yùn)動的估計(jì)精度，將式(1)中的旋轉(zhuǎn)矩陣可簡化為單位陣，即x2y2z2=ΔxΔyΔz]]>那么速度計(jì)算與特征點(diǎn)在圖像中的初始位置無關(guān)，只與前后幀圖像中特征點(diǎn)位置變化，如圖2所示。當(dāng)特征點(diǎn)為靜止時(shí)，利用雙目相機(jī)測得的景深信息與相對運(yùn)動原理，可將特征點(diǎn)在圖像中像素點(diǎn)的運(yùn)動估計(jì)結(jié)果解算出視覺采 集設(shè)備所在運(yùn)動載體的運(yùn)動速度。3、MEMS慣導(dǎo)輔助圖像匹配圖像匹配算法采用歸一化積相關(guān)算法，其本質(zhì)是一種兩幅圖像間的相似程度度量準(zhǔn)則。匹配算法的一般過程是，模板圖像在基準(zhǔn)圖像中從起始位置逐行掃過整個(gè)基準(zhǔn)圖；在每一個(gè)位置將模板圖像中每個(gè)像素點(diǎn)與它所對應(yīng)基準(zhǔn)圖區(qū)域像素點(diǎn)進(jìn)行相關(guān)性運(yùn)算，將結(jié)果記錄在相應(yīng)的位置處；選取相關(guān)性最高的匹配峰值點(diǎn)，作為匹配像素點(diǎn)。相關(guān)性運(yùn)算的計(jì)算公式為CC(u,v)=Σx,y[T(x,y)-T‾u,v][I(x+u,y+v)-I‾u,v]Σx,y[T(x,y)-T‾u,v]2Σx,y[I(x+u,y+v)-I‾u,v]2]]>式中，(x,y)為模板圖像中的坐標(biāo)點(diǎn)；(u,v)為基準(zhǔn)圖像中的坐標(biāo)點(diǎn)；T(x,y)為模板圖像在點(diǎn)(x,y)處的灰度值；為模板圖像在基準(zhǔn)圖像(u,v)處的均值；I(x+u,y+v)為基準(zhǔn)圖像點(diǎn)(u,v)處與模板圖像相對應(yīng)位置處的灰度值；為基準(zhǔn)圖像(u,v)處與模板圖像相同區(qū)域的均值。由上式可見，圖像匹配算法的計(jì)算復(fù)雜度是與搜索基準(zhǔn)圖像空間大小是相關(guān)的。為了降低算法的計(jì)算復(fù)雜度，引入MEMS慣導(dǎo)的姿態(tài)與速度信息，利用相鄰圖像采集周期內(nèi)的移動距離參考信息，以及特征點(diǎn)的景深信息，可估計(jì)出匹配特征點(diǎn)在匹配圖像中可能存在的區(qū)域。從而一方面降低了算法難度，另外也提高了匹配準(zhǔn)確度。4、多景深誤匹配點(diǎn)剔除技術(shù)利用k個(gè)不同景深的特征點(diǎn)計(jì)算出k個(gè)速度vl(l＝1,2,…,k)。但是由于匹配精度限制與圖像匹配中誤匹配的存在，部分速度vl計(jì)算并不準(zhǔn)確，相當(dāng)于速度計(jì)算噪聲，因此需要剔除誤匹配點(diǎn)計(jì)算速度的影響。本發(fā)明采用小波分析的方法來剔除速度噪聲影響。將k個(gè)特征點(diǎn)對應(yīng)計(jì)算速度vl(l＝1,2,…,k)設(shè)定為原始一維信號，將一維信號做二級小波變化，即設(shè)二級小波系數(shù)為w2＝[wa2wd2wd1]式中，wa2為二級近似系數(shù)；wd2為二級細(xì)節(jié)系數(shù)；wd1為一級細(xì)節(jié)系數(shù)。利用原始一維信號可計(jì)算二級小波系數(shù)，并對一維信號分別做二級近似分解、二級細(xì)節(jié)分解和一級細(xì)節(jié)分解獲得a2、d2和d1。去除誤匹配點(diǎn)計(jì)算速度噪聲相當(dāng)于是去除一維信號中的高頻信號的過程。對于二級小波而言，設(shè)定2個(gè)閾值λ1和λ2，認(rèn)為一級小波細(xì)節(jié)分解d1中小于λ1的部分為一級噪聲，而二級小波細(xì)節(jié)分解d2中小于λ2的部分為二級噪聲。將噪聲去除之后獲得一級小波細(xì)節(jié)d1`和二級小波細(xì)節(jié)d2`，恢復(fù)信號可獲得去除噪聲的信號vl(l＝1,2,…,k)，即v‾=a2+d1′+d2′]]>圖3為某條次利用本方法測量速度與GPS速度對比結(jié)果。當(dāng)前第1頁1 2 3