本發(fā)明涉及一種慣性測(cè)量裝置及方法，尤其涉及一種基于攝像頭陣列的慣性測(cè)量裝置及方法。

背景技術(shù)：

目前廣泛采用mems陀螺儀和mems加速度傳感器以及地磁傳感器組合進(jìn)行慣性測(cè)量。mems陀螺儀以微機(jī)械結(jié)構(gòu)為振動(dòng)件，通過(guò)測(cè)量旋轉(zhuǎn)產(chǎn)生的科氏加速度來(lái)獲得角加速度，將角加速度對(duì)時(shí)間積分得到角速度，因此由于積分誤差的存在使得陀螺儀測(cè)量角度的時(shí)候產(chǎn)生零點(diǎn)漂移。由此可見，mems陀螺儀雖然有較好的動(dòng)態(tài)特性，但是隨著使用時(shí)間的推移，誤差不斷積累導(dǎo)致測(cè)量結(jié)果錯(cuò)誤。mems加速度傳感器通過(guò)震動(dòng)件測(cè)量加速度從而獲得速度，由于重力加速度的存在，使得系統(tǒng)在三維空間做變速運(yùn)動(dòng)時(shí)不能區(qū)分重力加速度和外力加速度而出現(xiàn)錯(cuò)誤。地磁傳感器通過(guò)感應(yīng)地磁場(chǎng)分布變化來(lái)測(cè)量被測(cè)物體航向角，因而其在有外來(lái)磁場(chǎng)干擾的環(huán)境下測(cè)量會(huì)出現(xiàn)錯(cuò)誤。而且以上三種傳感器容易受噪聲干擾，尤其在惡劣的環(huán)境下，其測(cè)量結(jié)果往往偏差很大。mems加速度傳感器為mems陀螺儀提供指向地心的絕對(duì)基準(zhǔn)，地磁傳感器為mems陀螺儀提供指向北極的絕對(duì)基準(zhǔn)，以克服mems陀螺儀自身零點(diǎn)漂移的缺陷，因此在被測(cè)物體以較高的加速度運(yùn)動(dòng)的時(shí)候或者周圍環(huán)境存在較為嚴(yán)重的磁場(chǎng)干擾的情況下，目前常規(guī)的測(cè)量方法不能達(dá)到高精度慣性測(cè)量的要求。

隨著光學(xué)成像技術(shù)和半導(dǎo)體技術(shù)日趨成熟，攝像頭成本越來(lái)越低，分辨率和幀率越來(lái)越高，使得基于攝像頭陣列的慣性測(cè)量裝置的成本不斷降低，測(cè)量精度和靈敏度不斷提高。下面以分辨率為1920*1080，幀率為120fps，視角為90°的廣角攝像頭為例說(shuō)明本發(fā)明的測(cè)量精度：在平移運(yùn)動(dòng)中以10米內(nèi)的物體為特征點(diǎn)則具有毫米級(jí)的平移尺度分辨能力和千米每秒級(jí)別的最大速度測(cè)量能力。在旋轉(zhuǎn)運(yùn)動(dòng)中以10米內(nèi)的物體為特征點(diǎn)則具有0.045°的旋轉(zhuǎn)角度分辨能力和60πrad/s的最大角速度測(cè)量能力。

技術(shù)實(shí)現(xiàn)要素：

為了解決目前常規(guī)的慣性測(cè)量方法存在的缺陷，本發(fā)明提供一種能夠在被測(cè)物體以較高的加速度運(yùn)動(dòng)的時(shí)候或者周圍環(huán)境存在較為嚴(yán)重的磁場(chǎng)干擾的情況下正常工作且能消除積分誤差的慣性測(cè)量方法，提高了慣性測(cè)量的精確性和穩(wěn)定性。本發(fā)明提供一種利用攝像頭陣列對(duì)周圍環(huán)境圖像實(shí)時(shí)采集，通過(guò)分析圖像中多個(gè)特征點(diǎn)在攝像頭成像平面上的成像位置的變化情況進(jìn)行慣性測(cè)量的方法和裝置。由于攝像頭陣列被固定到整個(gè)測(cè)量對(duì)象上或者攝像頭陣列以已知的運(yùn)動(dòng)方式相對(duì)于整個(gè)測(cè)量對(duì)象運(yùn)動(dòng)，因此可以用攝像頭陣列的慣性測(cè)量結(jié)果表示出整個(gè)測(cè)量對(duì)象的慣性測(cè)量結(jié)果。慣性測(cè)量結(jié)果可以精確地提供角速度，角加速度，線速度，線加速度，運(yùn)動(dòng)軌跡等信息。

為解決上述技術(shù)問(wèn)題，本發(fā)明提供一種基于攝像頭陣列的慣性測(cè)量裝置，其特征是，包括：

攝像頭陣列單元，用于實(shí)時(shí)采集環(huán)境圖像；

特征點(diǎn)生成單元，與攝像頭陣列單元相連接，用于分析攝像頭陣列采集到的每一幀圖像并從中選取多組特征點(diǎn)，構(gòu)建特征點(diǎn)矩陣；

環(huán)境分析單元，與特征點(diǎn)生成單元相連接，通過(guò)分析同一時(shí)刻各特征點(diǎn)分別在多個(gè)攝像頭成像平面中的成像位置，計(jì)算各特征點(diǎn)相對(duì)于攝像頭陣列在環(huán)境中的實(shí)際位置；

運(yùn)動(dòng)分析單元，與環(huán)境分析單元相連接，根據(jù)各特征點(diǎn)在同一攝像頭成像平面不同幀中的成像位置變化情況計(jì)算出整個(gè)裝置的角速度、角加速度、線速度、線加速度和運(yùn)動(dòng)軌跡。

進(jìn)一步地，所述的攝像頭陣列單元包括兩個(gè)或兩個(gè)以上攝像頭以及紅外作用部件，多個(gè)攝像頭之間的相對(duì)位置和相對(duì)角度固定或者不固定。

進(jìn)一步地，所述的攝像頭陣列包括相組合的廣角攝像頭和窄角攝像頭。

一種基于攝像頭陣列的慣性測(cè)量方法，其特征是，包括以下步驟：

利用攝像頭陣列單元實(shí)時(shí)采集環(huán)境圖像；

利用特征點(diǎn)生成單元分析攝像頭陣列采集到的每一幀圖像并從中選取多組特征點(diǎn)，用于構(gòu)建特征點(diǎn)矩陣；

利用環(huán)境分析單元分析同一時(shí)刻各特征點(diǎn)分別在多個(gè)攝像頭成像平面中的成像位置，計(jì)算各特征點(diǎn)相對(duì)于攝像頭陣列在環(huán)境中的實(shí)際位置；

利用運(yùn)動(dòng)分析單元根據(jù)各特征點(diǎn)在同一攝像頭成像平面不同幀中的成像位置變化情況，計(jì)算出整個(gè)裝置的角速度、角加速度、線速度、線加速度、運(yùn)動(dòng)軌跡，達(dá)到慣性測(cè)量的目的。

進(jìn)一步地，所述特征點(diǎn)的個(gè)數(shù)至少大于4；所述特征點(diǎn)選取相對(duì)于環(huán)境靜止的點(diǎn)；不同特征點(diǎn)到攝像頭陣列之間的距離之差的絕對(duì)值大于1米。

進(jìn)一步地，所述的環(huán)境分析單元計(jì)算過(guò)程包括：依次從特征點(diǎn)生成單元在t0時(shí)刻構(gòu)建的特征點(diǎn)矩陣中抽取其中一個(gè)特征點(diǎn)數(shù)據(jù)代入環(huán)境分析關(guān)系式，生成對(duì)應(yīng)特征點(diǎn)的實(shí)際位置信息，待所有特征點(diǎn)計(jì)算完成后構(gòu)建特征點(diǎn)空間坐標(biāo)矩陣。

進(jìn)一步地，所述的環(huán)境分析關(guān)系式為：其中表示特征點(diǎn)在攝像頭成像平面上的成像位置；表示攝像頭在空間中的位置；表示特征點(diǎn)在空間中的位置，t為轉(zhuǎn)置矩陣標(biāo)志。

進(jìn)一步地，所述的運(yùn)動(dòng)分析單元計(jì)算過(guò)程包括：由已知的各特征點(diǎn)在空間中的實(shí)際位置信息，和在t1時(shí)刻各特征點(diǎn)在某一攝像頭成像平面上成像位置，與在t0時(shí)刻對(duì)應(yīng)特征點(diǎn)在同一攝像頭成像平面上成像位置變化關(guān)系，求得在t0時(shí)刻到t1時(shí)刻之間此攝像頭運(yùn)動(dòng)情況，包括平移坐標(biāo)和旋轉(zhuǎn)角度；由于此攝像頭與整個(gè)慣性測(cè)量裝置之間位置關(guān)系固定或與整個(gè)慣性測(cè)量裝置之間以已知的狀態(tài)運(yùn)動(dòng)，因此可得整個(gè)慣性測(cè)量裝置和測(cè)量對(duì)象的運(yùn)動(dòng)情況。

進(jìn)一步地，所述的位置變化關(guān)系由以下關(guān)系式表示：

其中表示此攝像頭平移角度；表示在t1時(shí)刻特征點(diǎn)在此攝像頭上的成像位置；表示特征點(diǎn)在空間中的位置，t是轉(zhuǎn)置矩陣標(biāo)志；θ為此攝像頭成像平面在t1時(shí)刻相對(duì)于t0時(shí)刻旋轉(zhuǎn)的角度。

本發(fā)明所達(dá)到的有益效果：

1.本發(fā)明采用攝像頭陣列進(jìn)行慣性測(cè)量，克服傳統(tǒng)mems陀螺儀和mems加速度傳感器進(jìn)行慣性測(cè)量的諸多缺點(diǎn)，可測(cè)精確測(cè)量被測(cè)對(duì)象的線速度、線加速度、角速度、角加速度、運(yùn)動(dòng)軌跡等；

2.本發(fā)明提供的慣性測(cè)量方法直接測(cè)量量是運(yùn)動(dòng)軌跡信息和旋轉(zhuǎn)角度信息，與傳統(tǒng)mems陀螺儀和mems加速度傳感器直接測(cè)量量為角加速度和加速度相比，不存在積分誤差的影響，故沒(méi)有零點(diǎn)漂移，靈敏度高，測(cè)量精確，受外界干擾??；

3.本發(fā)明通過(guò)圖像處理來(lái)進(jìn)行慣性測(cè)量，不受海拔、地磁、電磁波等影響，因此可應(yīng)用于惡劣環(huán)境下對(duì)被測(cè)對(duì)象進(jìn)行精確的慣性測(cè)量；

4.本發(fā)明攝像頭陣列安裝位置不固定，可根據(jù)具體應(yīng)用場(chǎng)合作相應(yīng)調(diào)整，達(dá)到最佳的測(cè)量效果。

附圖說(shuō)明

圖1是本發(fā)明的攝像頭陣列所采集環(huán)境圖像及選取的特征點(diǎn)示意圖(此處以雙攝像頭為例，但不局限于雙攝像頭)；

圖2是本發(fā)明環(huán)境分析單元工作原理示意圖(此處以雙攝像頭為例，但不局限于雙攝像頭)；

圖3是本發(fā)明運(yùn)動(dòng)分析單元工作原理示意圖。

具體實(shí)施方式

下面結(jié)合附圖對(duì)本發(fā)明作進(jìn)一步描述。以下實(shí)施例僅用于更加清楚地說(shuō)明本發(fā)明的技術(shù)方案，而不能以此來(lái)限制本發(fā)明的保護(hù)范圍。

如圖1所示，本發(fā)明提供一種利用攝像頭陣列對(duì)周圍環(huán)境圖像實(shí)時(shí)采集，通過(guò)分析圖像中多個(gè)特征點(diǎn)在攝像頭成像平面上的成像位置的變化情況進(jìn)行慣性測(cè)量的方法和裝置。包括攝像頭陣列單元，特征點(diǎn)生成單元，環(huán)境分析單元，運(yùn)動(dòng)分析單元。

攝像頭陣列單元由兩個(gè)或兩個(gè)以上攝像頭組成，用于實(shí)時(shí)采集環(huán)境圖像。特征點(diǎn)生成單元用于分析攝像頭陣列采集到的每一幀圖像并從中選取多組特征點(diǎn)。如圖1所示，通過(guò)分析在t0時(shí)刻每個(gè)攝像頭采集到的圖像，選定一定數(shù)量的特征點(diǎn)，在本示意圖中，選定每個(gè)方塊的頂點(diǎn)作為特征點(diǎn)，特征點(diǎn)選取數(shù)量隨具體環(huán)境改變，環(huán)境越復(fù)雜，要求特征點(diǎn)越多。將每個(gè)特征點(diǎn)在不同攝像頭成像平面上的成像位置坐標(biāo)構(gòu)建成特征點(diǎn)矩陣f。

其中，表示編號(hào)為nm的特征點(diǎn)在第i個(gè)攝像頭成像平面中成像位置的x軸坐標(biāo)值，表示編號(hào)為nm的特征點(diǎn)在第i個(gè)攝像頭成像平面中成像位置的y軸坐標(biāo)值。特征點(diǎn)選取有如下要求：特征點(diǎn)的個(gè)數(shù)至少大于4且盡可能多；特征點(diǎn)盡可能選取相對(duì)于環(huán)境靜止的點(diǎn)；不同特征點(diǎn)到攝像頭陣列之間的距離之差的絕對(duì)值大于1米。

環(huán)境分析單元通過(guò)分析同一時(shí)刻各特征點(diǎn)分別在多個(gè)攝像頭成像平面中的成像位置計(jì)算各特征點(diǎn)相對(duì)于攝像頭陣列在環(huán)境中的實(shí)際位置。具體計(jì)算過(guò)程如下所述：

依次從特征點(diǎn)生成單元在t0時(shí)刻構(gòu)建的特征點(diǎn)矩陣f中抽取其中一個(gè)特征點(diǎn)數(shù)據(jù)代入環(huán)境分析關(guān)系式。

為了表示空間中某一特征點(diǎn)u的實(shí)際位置，構(gòu)建如圖2所示直角坐標(biāo)系，此處以二維平面作類比說(shuō)明其計(jì)算過(guò)程，實(shí)際情況為三維空間。其中x軸與ab兩攝像頭成像平面重合，y軸為ab兩攝像頭透鏡連線的垂直平分線。表示點(diǎn)u的實(shí)際位置即計(jì)算u點(diǎn)的坐標(biāo)值(xu,yu)。由圖可知a攝像頭透鏡的中心點(diǎn)a的坐標(biāo)為(-s,h)，b攝像頭透鏡的中心點(diǎn)b的坐標(biāo)為(s,h)，空間中的特征點(diǎn)u在a攝像頭成像平面上的成像位置為d點(diǎn)，d點(diǎn)的坐標(biāo)為(-xd,0)，特征點(diǎn)u在b攝像頭成像平面上的成像位置為e點(diǎn)，e點(diǎn)的坐標(biāo)為(xe,0)。構(gòu)建矩陣

由下列環(huán)境分析關(guān)系式可解出u點(diǎn)的坐標(biāo)

其中表示特征點(diǎn)在攝像頭成像平面上的成像位置；表示攝像頭在空間中的位置；表示特征點(diǎn)在空間中的位置，t為轉(zhuǎn)置矩陣標(biāo)志。

通過(guò)以上計(jì)算方法可得出特征點(diǎn)矩陣中一個(gè)特征點(diǎn)對(duì)應(yīng)的實(shí)際位置信息，依次將每一個(gè)特征點(diǎn)代入上述計(jì)算過(guò)程生成特征點(diǎn)空間坐標(biāo)矩陣c。

其中，xnm表示編號(hào)為nm的特征點(diǎn)在空間中位置坐標(biāo)x軸的坐標(biāo)值，ynm表示編號(hào)為nm的特征點(diǎn)在空間中位置坐標(biāo)y軸的坐標(biāo)值，znm表示編號(hào)為nm的特征點(diǎn)在空間中位置坐標(biāo)z軸的坐標(biāo)值。

運(yùn)動(dòng)分析單元根據(jù)各特征點(diǎn)在同一攝像頭成像平面不同幀中的成像位置變化情況計(jì)算出整個(gè)裝置的角速度、角加速度、線速度、線加速度等，達(dá)到慣性測(cè)量的目的。具體計(jì)算過(guò)程如下所述：環(huán)境分析單元生成的特征點(diǎn)空間坐標(biāo)矩陣c對(duì)應(yīng)各個(gè)特征點(diǎn)在空間中的位置，由已知的各特征點(diǎn)在空間中的實(shí)際位置信息，和在t1時(shí)刻各特征點(diǎn)在a攝像頭成像平面上成像位置，與在t0時(shí)刻對(duì)應(yīng)特征點(diǎn)在a攝像頭成像平面上成像位置變化關(guān)系，求得在t0時(shí)刻到t1時(shí)刻之間a攝像頭運(yùn)動(dòng)情況，包括平移距離和旋轉(zhuǎn)角度；a攝像頭與整個(gè)慣性測(cè)量裝置之間位置關(guān)系固定，由此可得慣性測(cè)量裝置的運(yùn)動(dòng)情況。

如圖3所示，此處以二維平面作類比說(shuō)明其計(jì)算過(guò)程，實(shí)際情況為三維空間。在t0時(shí)刻，a攝像頭透鏡的中心a在直角坐標(biāo)系中的坐標(biāo)為(-s,h)，a攝像頭成像平面與x軸重合。在t1時(shí)刻，a攝像頭透鏡的中心a’在直角坐標(biāo)系中的坐標(biāo)為(-s+xm,h+ym)，即a攝像頭透鏡的中心在t1時(shí)刻相對(duì)于t0時(shí)刻平移(xm,ym)，a攝像頭成像平面與x軸夾角為θ，即a攝像頭成像平面在t1時(shí)刻相對(duì)于t0時(shí)刻旋轉(zhuǎn)θ。

構(gòu)建矩陣

則矩陣

可得到下列關(guān)系式

其中表示此攝像頭平移角度；表示在t1時(shí)刻特征點(diǎn)在此攝像頭上的成像位置；表示特征點(diǎn)在空間中的位置，t是轉(zhuǎn)置矩陣標(biāo)志；θ為此攝像頭成像平面在t1時(shí)刻相對(duì)于t0時(shí)刻旋轉(zhuǎn)的角度。

也就是說(shuō)分別將每一個(gè)特征點(diǎn)的空間位置坐標(biāo)、t0時(shí)刻此特征點(diǎn)在其中某一攝像頭成像平面中成像位置坐標(biāo)、t1時(shí)刻此特征點(diǎn)在同一攝像頭成像平面中成像位置坐標(biāo)代入關(guān)系式：可得一組含有4個(gè)未知數(shù)的等式組。4個(gè)未知數(shù)分別是t0時(shí)刻到t1時(shí)刻之間對(duì)應(yīng)攝像頭平移運(yùn)動(dòng)的x軸坐標(biāo)xm、y軸坐標(biāo)ym、z軸坐標(biāo)zm、和旋轉(zhuǎn)角度θ，剔除無(wú)效等式后解出以上4個(gè)未知數(shù)(無(wú)效等式主要由于在t0時(shí)刻到t1時(shí)刻之間特征點(diǎn)在環(huán)境中的位置發(fā)生了變化或者特征點(diǎn)識(shí)別錯(cuò)誤產(chǎn)生)，即可確定在t0時(shí)刻到t1時(shí)刻之間該裝置的運(yùn)動(dòng)情況。高幀率的攝像頭可使t0時(shí)刻到t1時(shí)刻的時(shí)間間隔很小，故可用平均線速度表示瞬時(shí)線速度，可用平均角速度表示瞬時(shí)角速度。上述t0時(shí)刻和t1時(shí)刻不局限于相鄰幀所對(duì)應(yīng)的時(shí)刻。

以上所述僅是本發(fā)明的優(yōu)選實(shí)施方式，應(yīng)當(dāng)指出，對(duì)于本技術(shù)領(lǐng)域的普通技術(shù)人員來(lái)說(shuō)，在不脫離本發(fā)明技術(shù)原理的前提下，還可以做出若干改進(jìn)和變形，這些改進(jìn)和變形也應(yīng)視為本發(fā)明的保護(hù)范圍。