專利名稱:基于單目視覺的鉆車姿態(tài)測(cè)量系統(tǒng)的制作方法
技術(shù)領(lǐng)域:
本實(shí)用新型涉及一種鉆車控制領(lǐng)域中的姿態(tài)測(cè)量系統(tǒng),特別涉及一種基于單目視覺的鉆車姿態(tài)測(cè)量系統(tǒng)。
背景技術(shù):
煤礦用液壓掘進(jìn)鉆車工作時(shí)需要明確鉆桿的姿態(tài)以便實(shí)現(xiàn)定向鉆進(jìn)。在實(shí)際操作過程中,通常使用價(jià)格昂貴的隨鉆測(cè)量系統(tǒng)測(cè)量鉆桿姿態(tài)。在多數(shù)非精確鉆進(jìn)應(yīng)用中,只需要已知鉆車自身姿態(tài),然后通過人工矯正的方式即可實(shí)現(xiàn)定向鉆進(jìn)。此方案與隨鉆測(cè)量系統(tǒng)相比,具有成本低、可靠性高等特點(diǎn)。目前尚未查閱到與此方案類似的專利和產(chǎn)品。測(cè)量姿態(tài)通常使用帶有三軸加速度計(jì)和三軸磁強(qiáng)計(jì)的姿態(tài)傳感器。但由于這類傳感器容易受到電磁干擾的影響,而鉆車自身具有強(qiáng)電磁干擾,因此傳統(tǒng)的姿態(tài)傳感器不能 直接安裝在鉆車上使用。必須使傳感器與鉆車相隔一定距離,以便降低鉆車的電磁干擾信號(hào)對(duì)傳感器的影響。
實(shí)用新型內(nèi)容本實(shí)用新型的目的在于克服上述不足,提供一種基于單目視覺的鉆車姿態(tài)測(cè)量系統(tǒng),避免傳感器受到鉆車電磁干擾,降低測(cè)量成本。本實(shí)用新型的目的是這樣實(shí)現(xiàn)的一種基于單目視覺的鉆車姿態(tài)測(cè)量系統(tǒng),該測(cè)量系統(tǒng)包括三軸加速度計(jì)、三軸磁強(qiáng)計(jì)、攝像機(jī)、輔助平臺(tái)、礦用工業(yè)計(jì)算機(jī)、鉆車以及由光點(diǎn)I、光點(diǎn)II、光點(diǎn)III與光點(diǎn)IV組成的光點(diǎn)矩陣所述三軸加速度計(jì)和三軸磁強(qiáng)計(jì)均安裝在輔助平臺(tái)上。所述攝像機(jī)固定在輔助平臺(tái)上方,攝像機(jī)的視野能夠完整的覆蓋光點(diǎn)矩形,所述礦用工業(yè)計(jì)算機(jī)中安裝圖像采集卡,所述攝像機(jī)輸出的模擬信號(hào)通過圖像采集卡輸入到礦用工業(yè)計(jì)算機(jī)中,所述光點(diǎn)I、光點(diǎn)II、光點(diǎn)III、光點(diǎn)IV通過安裝在鉆車上的干電池供電發(fā)光。與現(xiàn)有技術(shù)相比,本實(shí)用新型的有益效果本實(shí)用新型能夠?qū)︺@車姿態(tài)進(jìn)行非接觸式測(cè)量,避免鉆車電磁信號(hào)對(duì)姿態(tài)傳感器的干擾,系統(tǒng)使用的攝像機(jī)、礦用工業(yè)計(jì)算機(jī)以及姿態(tài)傳感器安裝方便,維護(hù)方便,造價(jià)較低,所使用到的姿態(tài)測(cè)量方法算法成熟,可靠性高,測(cè)量精度高。
圖I為本實(shí)用新型的鉆車姿態(tài)測(cè)量系統(tǒng)示意圖。圖2為本實(shí)用新型的輔助平臺(tái)部分的結(jié)構(gòu)示意圖。其中地理坐標(biāo)系I三軸加速度計(jì)2三軸磁強(qiáng)計(jì)3[0014]攝像機(jī)坐標(biāo)系4攝像機(jī)5輔助平臺(tái)坐標(biāo)系6輔助平臺(tái)7礦用工業(yè)計(jì)算機(jī)8光點(diǎn)矩形坐標(biāo)系9光點(diǎn)矩形10光點(diǎn)Ill光點(diǎn)II 12 光點(diǎn) III 13 光點(diǎn) IV 14鉆車坐標(biāo)系15鉆車16。
具體實(shí)施方式
本實(shí)用新型涉及一種基于單目視覺的鉆車姿態(tài)測(cè)量系統(tǒng)。測(cè)量系統(tǒng)結(jié)構(gòu)參見圖I一圖2所示,包括三軸加速度計(jì)2、三軸磁強(qiáng)計(jì)3、攝像機(jī)5、輔助平臺(tái)7、礦用工業(yè)計(jì)算機(jī)8、光點(diǎn)矩形10、光點(diǎn)I 11、光點(diǎn)II 12、光點(diǎn)III 13、光點(diǎn)IV 14和鉆車16。三軸加速度計(jì)2采用ADI公司ADXL系列產(chǎn)品,三軸磁強(qiáng)計(jì)3采用HONEYWELL公司HMC系列磁阻式磁強(qiáng)計(jì),攝像機(jī)5采用防爆攝像機(jī),輔助平臺(tái)7采用防爆機(jī)殼,礦用工業(yè)計(jì)算機(jī)8采用工業(yè)防爆計(jì)算機(jī),光點(diǎn)I 11、光點(diǎn)II 12、光點(diǎn)III 13和光點(diǎn)IV 14均使用高亮度發(fā)光二極管。三軸加速度計(jì)2和三軸磁強(qiáng)計(jì)3均安裝在輔助平臺(tái)7上,攝像機(jī)5固定在輔助平臺(tái)7上方,其視野能夠完整的覆蓋光點(diǎn)矩形10,礦用工業(yè)計(jì)算機(jī)8中安裝圖像采集卡,攝像機(jī)5輸出的模擬信號(hào)通過圖像采集卡輸入到礦用工業(yè)計(jì)算機(jī)8中。光點(diǎn)I 11、光點(diǎn)II 12、光點(diǎn)III 13和光點(diǎn)IV 14通過安裝在鉆車上的干電池供電發(fā)光。本實(shí)用新型涉及的一種基于單目視覺的鉆車姿態(tài)測(cè)量方法,所述測(cè)量方法包括以下步驟步驟一建立地理坐標(biāo)系1,以輔助平臺(tái)7附近某點(diǎn)為坐標(biāo)原點(diǎn),分別使地理坐標(biāo)系I的X軸、Y軸和Z軸指向地理東方、地理北方和地心;建立攝像機(jī)坐標(biāo)系4,以攝像機(jī)5的光心作為坐標(biāo)系原點(diǎn),以攝像機(jī)5光軸向外方向?yàn)閆軸正向,以成像平面水平向左方法為X軸正向,Y軸正方向通過右手法則確定;建立輔助平臺(tái)坐標(biāo)系6,以輔助平臺(tái)7的表面中心為原點(diǎn),以水平向右為X軸正方向,以輔助平臺(tái)7截面向里為Y軸正方向,Z軸正方向通過右手法則確定;建立光點(diǎn)矩形坐標(biāo)系9,以光點(diǎn)矩形10的中心為原點(diǎn),以光點(diǎn)矩形10水平向右為X軸正方向,光點(diǎn)矩形10水平向下為Y軸正方向,Z軸正方向通過右手法則確定;建立鉆車坐標(biāo)系15,以鉆車16的表面某一點(diǎn)為原點(diǎn),以鉆車16水平向前為X軸正方向,水平向右為Y軸正方向,Z軸正方向通過右手法則確定。步驟二 設(shè)置攝像機(jī)5的內(nèi)參數(shù),測(cè)量光點(diǎn)矩形10的長(zhǎng)度和寬度。步驟三利用三坐標(biāo)儀標(biāo)定攝像機(jī)坐標(biāo)系4與輔助平臺(tái)坐標(biāo)系6的相對(duì)姿態(tài)R1,光點(diǎn)矩形坐標(biāo)系9與鉆車坐標(biāo)系15的相對(duì)姿態(tài)R2。步驟四利用攝像機(jī)5采集光點(diǎn)圖像,以光點(diǎn)圖像坐標(biāo)作為特征點(diǎn),利用正交迭代方法和步驟二中獲得的攝像機(jī)內(nèi)參數(shù)求解P4P問題,計(jì)算得到光點(diǎn)矩形坐標(biāo)系9相對(duì)于攝像機(jī)坐標(biāo)系4的姿態(tài)R3 ;步驟五利用姿態(tài)傳感器測(cè)量輔助平臺(tái)坐標(biāo)系6相對(duì)于地理坐標(biāo)系I的姿態(tài)R4,具體步驟如下5. I、利用三軸加速度計(jì)2測(cè)量輔助平臺(tái)7所處位置的重力加速度矢量[gx, gy,gz]T,其中g(shù)x,gy和gz分別為三軸加速度計(jì)2的輸出值;5. 2、利用三軸磁強(qiáng)計(jì)3測(cè)量輔助平臺(tái)7所處位置的磁場(chǎng)強(qiáng)度[mx,my, mj,其中mx, my和mz分別為三軸磁強(qiáng)計(jì)3的輸出值。5. 3、利用下式計(jì)算R4
cos & cos lf/cos B if/— sin ^
= sin 爐sin ^coscos ^Psin iff sin g sm &sm i//+cos ^cossin ^pcosd cos <p$m 5cosy/+sin ^psm cos ^siri 0sm sin ^pcos cos 爐cos 5其中,
^=StanfgjlZg1)
& = -atan (g, / -Jgf+gf)
9= atan(gC^g)^-mygz)/(m,(gJy + g:)-myg而g為重力加速度值。步驟六通過坐標(biāo)變換計(jì)算鉆車坐標(biāo)系15相對(duì)于地理坐標(biāo)系I的姿態(tài)R R = R2tR3R1R4其中,R2T為姿態(tài)R2的轉(zhuǎn)置矩陣。
權(quán)利要求1. 一種基于單目視覺的鉆車姿態(tài)測(cè)量系統(tǒng),其特征在于該測(cè)量系統(tǒng)包括三軸加速度計(jì)(2)、三軸磁強(qiáng)計(jì)(3)、攝像機(jī)(5)、輔助平臺(tái)(7)、礦用工業(yè)計(jì)算機(jī)(8)、鉆車(16)以及由光點(diǎn)I (11)、光點(diǎn)II (12)、光點(diǎn)III (13)與光點(diǎn)IV (14)組成的光點(diǎn)矩形(10),所述三軸加速度計(jì)(2)和三軸磁強(qiáng)計(jì)(3)均安裝在輔助平臺(tái)(7)上,所述攝像機(jī)(5)固定在輔助平臺(tái)(7)上方,攝像機(jī)(5)的視野能夠完整的覆蓋光點(diǎn)矩形(10),所述礦用工業(yè)計(jì)算機(jī)(8)中安裝圖像采集卡,所述攝像機(jī)(5 )輸出的模擬信號(hào)通過圖像采集卡輸入到礦用工業(yè)計(jì)算機(jī)(8 )中,所述光點(diǎn)I (11)、光點(diǎn)II (12)、光點(diǎn)III (13)、光點(diǎn)IV (14)通過安裝在鉆車(16)上的干電池供電發(fā)光。
專利摘要本實(shí)用新型涉及一種基于單目視覺的鉆車姿態(tài)測(cè)量系統(tǒng),其特征在于該測(cè)量系統(tǒng)包括三軸加速度計(jì)(2)、三軸磁強(qiáng)計(jì)(3)、攝像機(jī)(5)、輔助平臺(tái)(7)、礦用工業(yè)計(jì)算機(jī)(8)、鉆車(16)以及由光點(diǎn)I(11)、光點(diǎn)II(12)、光點(diǎn)III(13)與光點(diǎn)IV(14)組成的光點(diǎn)矩形(10),所述三軸加速度計(jì)和三軸磁強(qiáng)計(jì)均安裝在輔助平臺(tái)上,攝像機(jī)的視野能夠完整的覆蓋光點(diǎn)矩形,所述礦用工業(yè)計(jì)算機(jī)中安裝圖像采集卡,所述攝像機(jī)輸出的模擬信號(hào)通過圖像采集卡輸入到礦用工業(yè)計(jì)算機(jī)中。本實(shí)用新型能夠?qū)︺@車姿態(tài)進(jìn)行非接觸式測(cè)量,避免鉆車電磁信號(hào)對(duì)姿態(tài)傳感器的干擾,所使用的測(cè)量方法算法成熟,可靠性高,測(cè)量精度高。
文檔編號(hào)G01C11/00GK202562485SQ20122007482
公開日2012年11月28日 申請(qǐng)日期2012年3月2日 優(yōu)先權(quán)日2012年3月2日
發(fā)明者余意, 楊平, 李遜 申請(qǐng)人:江陰中科礦業(yè)安全科技有限公司