專利名稱：基于立體視覺(jué)的速度測(cè)量方法
技術(shù)領(lǐng)域：
本發(fā)明屬于軌道交通技術(shù)領(lǐng)域，特別涉及一種基于立體視覺(jué)的速度測(cè)量方法。
背景技術(shù)：
速度是火車(chē)駕駛和安全控制的一個(gè)重要數(shù)據(jù)，其準(zhǔn)確性及可靠性決定列車(chē)的運(yùn)行 質(zhì)量。目前，列車(chē)的運(yùn)行速度主要是通過(guò)直接或間接地對(duì)車(chē)輪計(jì)數(shù)來(lái)測(cè)量，然后根據(jù)車(chē)輪周 長(zhǎng)及單位時(shí)間內(nèi)車(chē)輪的轉(zhuǎn)數(shù)計(jì)算出列車(chē)的速度。這種方法的準(zhǔn)確度受輪徑磨耗及輪軌間相 對(duì)滑動(dòng)的影響?，F(xiàn)有的一些復(fù)合測(cè)速方法雖然融合了一些傳感器的數(shù)據(jù)，但是基于車(chē)輪計(jì) 數(shù)的測(cè)量數(shù)據(jù)還是其進(jìn)行速度估計(jì)的主要依據(jù)，因此其準(zhǔn)確度仍然受輪徑磨耗及輪軌間相 對(duì)滑動(dòng)的影響。因此，基于車(chē)輪計(jì)數(shù)的測(cè)速方法雖然原理簡(jiǎn)單、容易實(shí)現(xiàn)，但是其測(cè)量精度容易受 到輪徑變化、車(chē)輪空轉(zhuǎn)、打滑影響。而且，空轉(zhuǎn)和打滑的時(shí)機(jī)以及由此引發(fā)的誤差很難用數(shù) 學(xué)的方法進(jìn)行描述，從而其影響也無(wú)法通過(guò)復(fù)雜的數(shù)學(xué)處理和融合濾波的方式消除。

發(fā)明內(nèi)容
(一 )要解決的技術(shù)問(wèn)題本發(fā)明要解決的技術(shù)問(wèn)題其一是，如何消除車(chē)輪空轉(zhuǎn)、打滑對(duì)速度測(cè)量精度的影 響；其二是如何實(shí)現(xiàn)低噪聲、實(shí)時(shí)的速度測(cè)量。( 二 )技術(shù)方案為解決上述技術(shù)問(wèn)題，本發(fā)明提供了一種基于立體視覺(jué)的速度測(cè)量方法，包括以 下步驟Si，實(shí)施基于視頻的速度計(jì)算方法采集載體運(yùn)行時(shí)前方靜止標(biāo)識(shí)物的圖像，根據(jù) 圖像信息在空間和時(shí)間上的相關(guān)性，通過(guò)在空間維度和時(shí)間維度對(duì)圖像進(jìn)行處理，得到載 體的運(yùn)行速度的測(cè)量值< ；S2，實(shí)施融合算法測(cè)量載體運(yùn)行軌道的坡度Φ的變化率ω，并測(cè)量載體運(yùn)行方 向的加速度ax，利用運(yùn)行速度的測(cè)量值<、變化率ω以及加速度ax建立進(jìn)行載體運(yùn)行速度 估計(jì)的過(guò)程模型和測(cè)量模型，然后根據(jù)該過(guò)程模型和測(cè)量模型利用卡爾曼濾波算法估計(jì)出 載體的運(yùn)行速度作為最終結(jié)果。其中，在步驟Sl中，利用兩個(gè)攝像頭采集標(biāo)識(shí)物的圖像，該兩個(gè)攝像頭平行放置， 構(gòu)成雙目立體視覺(jué)系統(tǒng)，且兩個(gè)攝像頭均安裝在車(chē)頭，視安裝高度，調(diào)整使其相對(duì)水平線的 夾角在5度到85度之間。其中，在步驟S2中，利用陀螺儀測(cè)量載體運(yùn)行軌道的坡度Φ的變化率ω，利用加 速度計(jì)測(cè)量載體運(yùn)行方向的加速度ax。其中，步驟Sl進(jìn)一步包括以下步驟S11，在時(shí)刻^對(duì)載體運(yùn)行時(shí)前方靜止標(biāo)識(shí)物進(jìn)行視頻采樣，得到兩個(gè)攝像頭在同 一時(shí)刻對(duì)相同標(biāo)識(shí)物的圖像；
S12，在其中一幅圖像中選擇特征點(diǎn)Pi,所述特征點(diǎn)是指相對(duì)來(lái)說(shuō)邊界明顯、對(duì)比 清晰、且易于跟蹤和識(shí)別的像素區(qū)域中的點(diǎn)；S13，利用步驟S12中選出的特征點(diǎn)Pi,在另一幅圖像尋找相匹配的特征點(diǎn)P'
形成同一特征點(diǎn)在兩幅圖像中的匹配對(duì)集合{Pi，i = 1,2,... ,N}和{P' pi = 1,2,...,
N}；S14，根據(jù)兩個(gè)攝像頭的位置間距b以及步驟S13所得到的匹配對(duì)集合，計(jì)算特征 點(diǎn)Pi在攝像頭坐標(biāo)系中的位置Xi，并記錄特征點(diǎn)Pi及其坐標(biāo)位置{Pi，XiI ；S15，在時(shí)刻t2進(jìn)行視頻采樣；S16，同樣利用步驟S12中得到的特征點(diǎn)的集合，在同一攝像頭在時(shí)刻t2所攝圖像 中進(jìn)行匹配查找，得到匹配的特征點(diǎn)集合=；S17，利用步驟S16所得的結(jié)果在t2時(shí)刻另一攝像頭所攝圖像中進(jìn)行匹配，得到相 應(yīng)的匹配點(diǎn)集合|巧，/ = 1，2,...，_ ;按步驟S14中的方法計(jì)算出步驟S12確定的特征點(diǎn)？1在 時(shí)刻t2在攝像頭坐標(biāo)系下的位置{Pi, Xi' }；S18，根據(jù)步驟S14和S17所得的結(jié)果以及相鄰兩次視頻采樣的時(shí)間間隔，計(jì)算出 特征點(diǎn)在攝像頭坐標(biāo)系下的速度弋=7^7.
ι ι ’S19，根據(jù)兩個(gè)攝像頭安裝的幾何位置進(jìn)行坐標(biāo)變換，得到特征點(diǎn)在載體坐標(biāo)系下 的速度，計(jì)算出的速度即為載體的運(yùn)行速度；S10，計(jì)算同一時(shí)刻所有特征點(diǎn)的速度，取其平均值作為該時(shí)刻載體的運(yùn)行速度V， 然后取運(yùn)行速度ν在載體運(yùn)行方向的分量<作為載體運(yùn)行速度的測(cè)量值。其中，步驟S2進(jìn)一步包括以下步驟S21，利用單軸陀螺儀測(cè)量軌道的坡度Φ的變化率ω，并利用單軸加速度計(jì)測(cè)量
載體運(yùn)行方向的加速度ax，將單軸陀螺儀和單軸加速度計(jì)的測(cè)量模型分別用下式描述 ‘■
φ=ω+ +υ =0
(1)
其中，b為單軸陀螺儀的測(cè)量偏差，υ為單軸陀螺儀的測(cè)量噪聲，g為重力加速 度，η為單軸加速度計(jì)的測(cè)量噪聲，采用高斯白噪聲模型表示兩種測(cè)量噪聲υ N(0，Q)， η N(0，R)，其中E[U ‘ υ] =Q,E[n' η] =R，E [*]表示期望值，其根據(jù)實(shí)際測(cè)量數(shù) 據(jù)統(tǒng)計(jì)得到，上標(biāo)“，，，表示轉(zhuǎn)置運(yùn)算；S22，利用步驟Sl得到的測(cè)量值將載體速度表示為V^ =Vx+ε(2)其中ε為測(cè)量誤差，ε Ν(0，H)，其中Ε[ ε ‘ ε] = H，H根據(jù)實(shí)際測(cè)量數(shù)據(jù)統(tǒng) 計(jì)得到，Vx為載體速度的真實(shí)值；S23，將式(1)線性化為
6
式(3)和式(4)分別是利用卡爾曼濾波算法進(jìn)行速度估計(jì)的過(guò)程模型和測(cè)量模 型，根據(jù)該過(guò)程模型和測(cè)量模型利用卡爾曼濾波算法估計(jì)出載體的運(yùn)行速度。其中，根據(jù)該過(guò)程模型和測(cè)量模型利用卡爾曼濾波算法估計(jì)出載體的運(yùn)行速度的 過(guò)程如下(1)利用所選取的采樣頻率仁分別將式(3)、(4)離散化，得到離散化模型，其中采 樣頻率fs大于IOHz ；(2)根據(jù)(1)中得到的離散化模型，和統(tǒng)計(jì)得到的單軸陀螺儀和單軸加速度計(jì)的 測(cè)量噪聲的噪聲方差，利用標(biāo)準(zhǔn)的穩(wěn)態(tài)卡爾曼濾波算法計(jì)算得到卡爾曼濾波器；(3)在每個(gè)采樣時(shí)刻執(zhí)行步驟a 讀取單軸加速度計(jì)及單軸陀螺儀的測(cè)量值，然后 把同一采樣時(shí)刻步驟Sl和步驟a的測(cè)量值輸入到卡爾曼濾波器，經(jīng)過(guò)濾波處理，得到的輸 出作為所述最終結(jié)果。
波算法
其中，利用Matlab軟件控制工具箱的卡爾曼命令實(shí)現(xiàn)所述標(biāo)準(zhǔn)的穩(wěn)態(tài)卡爾曼濾
其中，所述采樣頻率fs大于IOHz。 其中，所述采樣頻率fs優(yōu)選30Hz。 其中，所述載體優(yōu)選為列車(chē)。 (三)有益效果
本發(fā)明能產(chǎn)生如下有益效果利用雙攝像頭構(gòu)成立體視覺(jué)配置，安裝于載體前部， 通過(guò)拍攝載體行進(jìn)前方靜態(tài)場(chǎng)景實(shí)現(xiàn)對(duì)速度的推算；基于視頻的速度估計(jì)算法利用圖像的 空間關(guān)聯(lián)對(duì)特征點(diǎn)進(jìn)行定位，利用特征點(diǎn)的時(shí)間關(guān)聯(lián)性進(jìn)行跟蹤。通過(guò)對(duì)相鄰連續(xù)幀的處 理，計(jì)算得到載體的速度。以上測(cè)量方法不依賴于對(duì)輪軸的計(jì)數(shù)，從而消除了車(chē)輪空轉(zhuǎn)、打 滑對(duì)測(cè)量精度的影響。進(jìn)一步，利用Kalman(卡爾曼)濾波的融合算法實(shí)現(xiàn)載體速度測(cè)量 值與加速度計(jì)、陀螺儀測(cè)量值的融合，實(shí)現(xiàn)了低噪聲、實(shí)時(shí)的速度測(cè)量。另外，測(cè)量裝置安裝 簡(jiǎn)單，維護(hù)方便。


圖1是本發(fā)明實(shí)施例的方法流程圖；圖2是本發(fā)明實(shí)施例的方法所使用的系統(tǒng)構(gòu)成框圖；圖3是實(shí)施本發(fā)明實(shí)施例的方法時(shí)攝像頭安裝位置及角度示意圖；圖4是本發(fā)明實(shí)施例的方法中基于視頻的速度計(jì)算方法原理示意圖。
具體實(shí)施例方式下面結(jié)合附圖和實(shí)施例，對(duì)本發(fā)明的具體實(shí)施方式
作進(jìn)一步詳細(xì)描述。以下實(shí)施 例用于說(shuō)明本發(fā)明，但不用來(lái)限制本發(fā)明的范圍。本發(fā)明實(shí)施例以列車(chē)為例進(jìn)行說(shuō)明。本發(fā)明實(shí)施例的方法流程圖如圖1所示。 如圖2所示，本發(fā)明的方法利用如下幾個(gè)裝置構(gòu)成的系統(tǒng)實(shí)現(xiàn)2個(gè)CCD (Charge-coupled Device,電荷華禹合兀件)(或 CMOS (Complementary metal-oxi de-semi conductor,互補(bǔ)性 氧化金屬半導(dǎo)體))攝像頭1、2、單軸加速度計(jì)、單軸陀螺儀。其中，單軸加速度計(jì)測(cè)量列 車(chē)運(yùn)行方向的加速度，單軸陀螺儀用來(lái)測(cè)量列車(chē)運(yùn)行軌道的坡度的變化率。兩個(gè)攝像頭平 行配置，構(gòu)成雙目立體視覺(jué)系統(tǒng)，完成對(duì)車(chē)輛運(yùn)行前方視景的實(shí)時(shí)捕獲。系統(tǒng)還包括兩個(gè) DSP (Digital Signal Processor，數(shù)字信號(hào)處理器)，其一是視頻處理DSP，負(fù)責(zé)視頻運(yùn)算處 理，另一個(gè)是速度計(jì)算DSP，用于運(yùn)行速度測(cè)量算法。如圖3所示，兩個(gè)攝像頭安裝在車(chē)頭，視安裝高度，調(diào)整使其相對(duì)水平線的夾角在 5度到85度之間，這樣保證攝像頭捕獲的圖像不包括車(chē)本身的圖像。本發(fā)明實(shí)施例的方法包括兩個(gè)主要步驟，第一，依據(jù)攝像頭采集的圖像信息，基于 視頻的速度計(jì)算方法得到速度的測(cè)量值；第二，結(jié)合加速度計(jì)和陀螺儀進(jìn)行數(shù)據(jù)融合，利用 Kalman濾波算法估計(jì)出列車(chē)速度?；谝曨l的速度估計(jì)算法是根據(jù)圖像信息在空間和時(shí)間上的相關(guān)性，通過(guò)在空間 維度和時(shí)間維度對(duì)圖像進(jìn)行相關(guān)運(yùn)算，推算出列車(chē)的運(yùn)行速度。原理說(shuō)明如圖3所示。圖4中，A、B、C、D為同一物體或標(biāo)識(shí)物分別在在不同攝像頭、不同時(shí)刻的映像。A 和B為分別表示在時(shí)刻1時(shí)同一標(biāo)識(shí)物在兩個(gè)攝像頭中的映像；C和D則分別表示這一標(biāo) 識(shí)物在時(shí)刻2時(shí)在兩個(gè)攝像頭中的映像。依據(jù)同一時(shí)刻的空間關(guān)聯(lián)，可以計(jì)算出標(biāo)識(shí)物在 攝像頭坐標(biāo)系下的位置，而依據(jù)時(shí)間上的關(guān)聯(lián)，則可以計(jì)算出在時(shí)刻1和時(shí)刻2間的這段時(shí) 間載體的平均運(yùn)動(dòng)速度。當(dāng)按視頻的采集頻率(通常為30Hz)進(jìn)行連續(xù)運(yùn)算處理時(shí)，便可 得到載體的即時(shí)速度。算法的處理步驟如下1，在時(shí)刻Kt1)進(jìn)行視頻采樣，得到攝像頭1和攝像頭2在同一時(shí)刻對(duì)相同場(chǎng)景 的快照。2，在其中一幅圖像中(如攝像頭1所攝圖像)尋找和選擇特征點(diǎn)Pi,特征點(diǎn)是那 些邊界明顯、對(duì)比清晰，易于跟蹤和識(shí)別的像素區(qū)域中的點(diǎn)。3，利用步驟2中選出的特征點(diǎn)Pi,在另一幅圖像尋找相匹配的特征點(diǎn)P' i，形成 同一特征點(diǎn)在兩幅圖像中的匹配對(duì)集合{Pi，i = 1，2，...，N}和{P' pi = 1，2，...，N}。4，根據(jù)兩攝像頭的位置間距b以及步驟3所形成的匹配集合，計(jì)算特征點(diǎn)在攝 像頭坐標(biāo)系中的位置Xi。位置Xi的計(jì)算采用文獻(xiàn)[1] (Milan Sonka, Vaclav Hlavac, Roger Boyle Image Processing, Analysis and Machine Vision, Second Edition, ISBN 0-534-95393-X, Brooks/Cole, pp. 460.)中的立體視覺(jué)算法(Stereo vision algorithm) 進(jìn)行。記錄特征點(diǎn)Pi及其坐標(biāo)位置{Py xj。5，在時(shí)刻2 (t2)進(jìn)行視頻采樣。6，同樣利用步驟2中得到的特征點(diǎn)集合，在該攝像頭在時(shí)刻2所攝圖像中進(jìn)行匹 配查找，得到匹配的特征點(diǎn)集合0，/ = 1，2，...，對(duì)。7，利用6中結(jié)果在t2時(shí)刻另一攝像頭所攝圖像中進(jìn)行匹配，得到相應(yīng)的匹配點(diǎn)集合·(巧，i = 1,2，...,JV}。按步驟4中同樣方法計(jì)算出步驟2確定的特征點(diǎn)Pi在時(shí)刻2時(shí)在攝像 頭坐標(biāo)系下的位置{Pi; χ/ }。 8，根據(jù)步驟4和7所得結(jié)果以及兩次相鄰視頻采樣間時(shí)間間隔，可以計(jì)算出特征
點(diǎn)Pi在攝像頭坐標(biāo)系下的速度< = 9，根據(jù)攝像頭安裝的幾何關(guān)系進(jìn)行坐標(biāo)變換，可以得到特征點(diǎn)Pi在列車(chē)坐標(biāo)系下 的速度。由于攝像頭所視場(chǎng)景為靜止場(chǎng)景，則計(jì)算出的速度大小即為列車(chē)的運(yùn)行速度。 cos θ sin θ -sin θ cos θ
10，計(jì)算所有特征點(diǎn)的速度，取其平均值作為該時(shí)刻列車(chē)的速度
θ的定義見(jiàn)圖;L 由于列在軌道上運(yùn)行，我們關(guān)注其前進(jìn)的速度。所以取ν的X方向分量< (列車(chē) 坐標(biāo)系參見(jiàn)圖3)作為列車(chē)速度的測(cè)量值。11，返回步驟1繼續(xù)運(yùn)行。根據(jù)上面步驟，可得到利用視頻圖像計(jì)算出的列車(chē)速度。由于視頻處理受攝像頭 采樣率及DSP處理能力影響，計(jì)算出的速度會(huì)存在延時(shí)。為了改善延時(shí)，減小噪聲，由視頻 處理得出的速度和加速度計(jì)及陀螺儀利用Kalman濾波算法進(jìn)行進(jìn)一步融合，從而達(dá)到即 時(shí)、低噪、準(zhǔn)確的速度估計(jì)。過(guò)程如下陀螺儀測(cè)量軌道水平坡度Φ的變化率ω。加速度計(jì)測(cè)量列車(chē)運(yùn)行方向的加速度
ax。考慮到噪聲和測(cè)量偏差，陀螺儀和加速度計(jì)的測(cè)量模型可分別由下式描述。 ‘■ 其中，等式左邊的符號(hào)中，變量上方加“ · ”表示對(duì)該變量微分，b為陀螺儀測(cè)量偏 差，g為重力加速度，υ和η分別為兩個(gè)傳感器的測(cè)量噪聲，我們采用高斯白噪聲模型， 其中Ε[υ' υ] =Q,E[n' η ]= R，根據(jù)實(shí)際測(cè)量數(shù)據(jù)統(tǒng)計(jì)得到。利用基于視頻的速度計(jì)算方法計(jì)算出的測(cè)量值將列車(chē)速度表示為K =νχ+ε(2)其中ε為測(cè)量誤差，ε Ν(0，Η)，Ε[ε ‘ ε ] = H，Vx為列車(chē)速度真實(shí)值。由于軌道坡度通常不超過(guò)10% (5.7° )，式(1)可以線性化。
_ . - Φ_ 010""10"ω"10"
b=000b+00H-00rs00Λ-0101
(3)
9
式(3)和式(4)分別是基于視覺(jué)速度估計(jì)的過(guò)程模型和測(cè)量模型，根據(jù)此模型可 以利用穩(wěn)態(tài)Kalman濾波算法，估計(jì)出列車(chē)的運(yùn)行速度。具體過(guò)程如下(1)選定計(jì)算采樣頻率fs，將模型式(3)、(4)離散化，得到離散化數(shù)學(xué)模型。為保 證測(cè)量精度，fs應(yīng)大于IOHz，通常選擇30Hz。(2)利用⑴中得到的離散化模型，和統(tǒng)計(jì)得到的噪聲方差，利用標(biāo)準(zhǔn)的穩(wěn)態(tài) Kalman濾波算法(如利用Matlab軟件控制工具箱的kalman命令)計(jì)算的到Kalman濾波
ο(3)在每個(gè)采樣時(shí)刻b，讀取加速度傳感器及陀螺儀測(cè)量數(shù)據(jù)；C，把基于視頻的 速度計(jì)算方法計(jì)算出的測(cè)量值和b的結(jié)果輸入到kalman濾波器，經(jīng)過(guò)濾波處理，得到的輸 出作為列車(chē)速度測(cè)量結(jié)果。本發(fā)明的方法不僅可用于列車(chē)測(cè)速也可用于對(duì)其它載體進(jìn)行測(cè)速，只是由于鐵路 運(yùn)行環(huán)境比較單一，因此比較適合應(yīng)用視覺(jué)方法。以上實(shí)施方式僅用于說(shuō)明本發(fā)明，而并非對(duì)本發(fā)明的限制，有關(guān)技術(shù)領(lǐng)域的普通 技術(shù)人員，在不脫離本發(fā)明的精神和范圍的情況下，還可以做出各種變化和變型，因此所有 等同的技術(shù)方案也屬于本發(fā)明的范疇，本發(fā)明的專利保護(hù)范圍應(yīng)由權(quán)利要求限定。
權(quán)利要求
一種基于立體視覺(jué)的速度測(cè)量方法，其特征在于，包括以下步驟S1，實(shí)施基于視頻的速度計(jì)算方法采集載體運(yùn)行時(shí)前方靜止標(biāo)識(shí)物的圖像，根據(jù)圖像信息在空間和時(shí)間上的相關(guān)性，通過(guò)在空間維度和時(shí)間維度對(duì)圖像進(jìn)行處理，得到載體的運(yùn)行速度的測(cè)量值S2，實(shí)施融合算法測(cè)量載體運(yùn)行軌道的坡度φ的變化率ω，并測(cè)量載體運(yùn)行方向的加速度ax，利用運(yùn)行速度的測(cè)量值變化率ω以及加速度ax建立進(jìn)行載體運(yùn)行速度估計(jì)的過(guò)程模型和測(cè)量模型，然后根據(jù)該過(guò)程模型和測(cè)量模型利用卡爾曼濾波算法估計(jì)出載體的運(yùn)行速度作為最終結(jié)果。FSA00000220994200011.tif,FSA00000220994200012.tif
2.如權(quán)利要求1所述的基于立體視覺(jué)的速度測(cè)量方法，其特征在于，在步驟Sl中，利用 兩個(gè)攝像頭采集標(biāo)識(shí)物的圖像，該兩個(gè)攝像頭平行放置，構(gòu)成雙目立體視覺(jué)系統(tǒng)，且兩個(gè)攝 像頭均安裝在車(chē)頭，視安裝高度，調(diào)整使其相對(duì)水平線的夾角在5度到85度之間。
3.如權(quán)利要求1或2所述的基于立體視覺(jué)的速度測(cè)量方法，其特征在于，在步驟S2中， 利用陀螺儀測(cè)量載體運(yùn)行軌道的坡度Φ的變化率ω，利用加速度計(jì)測(cè)量載體運(yùn)行方向的 加速度ax。
4.如權(quán)利要求3所述的基于立體視覺(jué)的速度測(cè)量方法，其特征在于，步驟Sl進(jìn)一步包 括以下步驟S11，在時(shí)刻^對(duì)載體運(yùn)行時(shí)前方靜止標(biāo)識(shí)物進(jìn)行視頻采樣，得到兩個(gè)攝像頭在同一時(shí) 刻對(duì)相同標(biāo)識(shí)物的圖像；S12，在其中一幅圖像中選擇特征點(diǎn)Pi,所述特征點(diǎn)是指相對(duì)來(lái)說(shuō)邊界明顯、對(duì)比清晰、 且易于跟蹤和識(shí)別的像素區(qū)域中的點(diǎn)；S13，利用步驟S12中選出的特征點(diǎn)Pi,在另一幅圖像尋找相匹配的特征點(diǎn)P' i，形成 同一特征點(diǎn)在兩幅圖像中的匹配對(duì)集合{Pi，i = 1，2，...，N}和{P' pi = 1，2，...，N}； S14，根據(jù)兩個(gè)攝像頭的位置間距b以及步驟S13所得到的匹配對(duì)集合，計(jì)算特征點(diǎn)Pi 在攝像頭坐標(biāo)系中的位置Xi，并記錄特征點(diǎn)Pi及其坐標(biāo)位置{Pi，Xi}； S15，在時(shí)刻t2進(jìn)行視頻采樣；S16，同樣利用步驟S12中得到的特征點(diǎn)的集合，在同一攝像頭在時(shí)刻t2所攝圖像中進(jìn)行匹配查找，得到匹配的特征點(diǎn)集合丨。=U，...，iV}；S17，利用步驟S16所得的結(jié)果在t2時(shí)刻另一攝像頭所攝圖像中進(jìn)行匹配，得到相應(yīng)的匹配點(diǎn)集合= 1,2,...,TV);按步驟S14中的方法計(jì)算出步驟S12確定的特征點(diǎn)Pi在時(shí)刻t2在攝像頭坐標(biāo)系下的位置{Pi，Xi' }；S18，根據(jù)步驟S14和S17所得的結(jié)果以及相鄰兩次視頻采樣的時(shí)間間隔，計(jì)算出特征點(diǎn)在攝像頭坐標(biāo)系下的速度< =^u 2 ‘ j · ？S19，根據(jù)兩個(gè)攝像頭安裝的幾何位置進(jìn)行坐標(biāo)變換，得到特征點(diǎn)在載體坐標(biāo)系下的速 度，計(jì)算出的速度即為載體的運(yùn)行速度；S10，計(jì)算同一時(shí)刻所有特征點(diǎn)的速度，取其平均值作為該時(shí)刻載體的運(yùn)行速度ν，然后 取運(yùn)行速度ν在載體運(yùn)行方向的分量<作為載體運(yùn)行速度的測(cè)量值。
5.如權(quán)利要求4所述的基于立體視覺(jué)的速度測(cè)量方法，其特征在于，步驟S2進(jìn)一步包括以下步驟S21，利用單軸陀螺儀測(cè)量軌道的坡度Φ的變化率ω，并利用單軸加速度計(jì)測(cè)量載體 運(yùn)行方向的加速度ax，將單軸陀螺儀和單軸加速度計(jì)的測(cè)量模型分別用下式描述 φ = ω + b + u b = 0+(工)其中，b為單軸陀螺儀的測(cè)量偏差，υ為單軸陀螺儀的測(cè)量噪聲，g為重力加速度，η 為單軸加速度計(jì)的測(cè)量噪聲，采用高斯白噪聲模型表示兩種測(cè)量噪聲υ N(0，Q)，n N(0，R)，其中E[u ‘ υ] =Q, E[n' n] = R，E [*]表示期望值，其根據(jù)實(shí)際測(cè)量數(shù)據(jù)統(tǒng) 計(jì)得到，上標(biāo)“ ’”表示轉(zhuǎn)置運(yùn)算；S22，利用步驟Sl得到的測(cè)量值將載體速度表示為(2)其中ε為測(cè)量誤差，ε Ν(0，H)，其中Ε[ ε ‘ ε] = H，H根據(jù)實(shí)際測(cè)量數(shù)據(jù)統(tǒng)計(jì)得 到，Vx為載體速度的真實(shí)值； S23，將式(1)線性化為(3)'φ“010"'Φ1"10""10"ωυb=000b+00+00.S00_0101Jl<二[0 0+ ε3」⑷式(3)和式(4)分別是利用卡爾曼濾波算法進(jìn)行速度估計(jì)的過(guò)程模型和測(cè)量模型，根 據(jù)該過(guò)程模型和測(cè)量模型利用卡爾曼濾波算法估計(jì)出載體的運(yùn)行速度。
6.如權(quán)利要求5所述的基于立體視覺(jué)的速度測(cè)量方法，其特征在于，根據(jù)該過(guò)程模型 和測(cè)量模型利用卡爾曼濾波算法估計(jì)出載體的運(yùn)行速度的過(guò)程如下(1)利用所選取的采樣頻率4分別將式(3)、(4)離散化，得到離散化模型，其中采樣頻 率fs大于IOHz ；(2)根據(jù)(1)中得到的離散化模型，和統(tǒng)計(jì)得到的單軸陀螺儀和單軸加速度計(jì)的測(cè)量 噪聲的噪聲方差，利用標(biāo)準(zhǔn)的穩(wěn)態(tài)卡爾曼濾波算法計(jì)算得到卡爾曼濾波器；(3)在每個(gè)采樣時(shí)刻執(zhí)行步驟a讀取單軸加速度計(jì)及單軸陀螺儀的測(cè)量值，然后把同 一采樣時(shí)刻步驟Sl和步驟a的測(cè)量值輸入到卡爾曼濾波器，經(jīng)過(guò)濾波處理，得到的輸出作 為所述最終結(jié)果。
7.如權(quán)利要求6所述的基于立體視覺(jué)的速度測(cè)量方法，其特征在于，利用Matlab軟件 控制工具箱的卡爾曼命令實(shí)現(xiàn)所述標(biāo)準(zhǔn)的穩(wěn)態(tài)卡爾曼濾波算法。
8.如權(quán)利要求6所述的基于立體視覺(jué)的速度測(cè)量方法，其特征在于，所述采樣頻率fs 大于IOHz。
9.如權(quán)利要求8所述的基于立體視覺(jué)的速度測(cè)量方法，其特征在于，所述采樣頻率fs取 30Hz。
10.如權(quán)利要求1 9所述的基于立體視覺(jué)的速度測(cè)量方法，其特征在于，所述載體為 列車(chē)。
全文摘要
本發(fā)明公開(kāi)了一種基于立體視覺(jué)的速度測(cè)量方法，包括以下步驟S1，采集載體運(yùn)行時(shí)前方靜止標(biāo)識(shí)物的圖像，根據(jù)圖像信息在空間和時(shí)間上的相關(guān)性，通過(guò)在空間維度和時(shí)間維度對(duì)圖像進(jìn)行處理，得到載體的運(yùn)行速度的測(cè)量值；S2，測(cè)量載體運(yùn)行軌道的坡度φ的變化率ω，并測(cè)量載體運(yùn)行方向的加速度ax，利用運(yùn)行速度的測(cè)量值、變化率ω以及加速度ax建立進(jìn)行載體運(yùn)行速度估計(jì)的過(guò)程模型和測(cè)量模型，然后根據(jù)該過(guò)程模型和測(cè)量模型利用卡爾曼濾波算法估計(jì)出載體的運(yùn)行速度作為最終結(jié)果。本發(fā)明的方法能夠消除車(chē)輪空轉(zhuǎn)、打滑對(duì)速度測(cè)量精度的影響；而且能夠?qū)崿F(xiàn)低噪聲、實(shí)時(shí)的速度測(cè)量。
文檔編號(hào)G01P3/68GK101915852SQ20101024802
公開(kāi)日2010年12月15日 申請(qǐng)日期2010年8月6日 優(yōu)先權(quán)日2010年8月6日
發(fā)明者于振宇, 劉波, 唐濤, 郜春海 申請(qǐng)人:北京交通大學(xué)