一種懸臂式掘進(jìn)機(jī)空間位姿實時檢測系統(tǒng)及方法
【專利摘要】本發(fā)明提出一種懸臂式掘進(jìn)機(jī)空間位姿實時檢測系統(tǒng)及方法，包括兩臺十字激光發(fā)射器、工控計算機(jī)、兩臺網(wǎng)絡(luò)攝像機(jī)、兩個激光標(biāo)靶。使用機(jī)器視覺技術(shù)作為位姿參數(shù)的采集方式，通過建立掘進(jìn)機(jī)位姿解算數(shù)學(xué)模型，得到掘進(jìn)機(jī)行進(jìn)過程中的偏航角、俯仰角、橫滾角以及機(jī)身上固定一點(diǎn)在巷道截面上的偏移量，即掘進(jìn)機(jī)的空間位姿，可用于掘進(jìn)機(jī)自動行進(jìn)中的糾偏控制。實驗結(jié)果表明該系統(tǒng)所檢測角度的精度在0.5°范圍內(nèi)，位移的精度優(yōu)于20mm，且每幀圖像的處理與顯示速度約為5s，滿足巷道施工過程中自動、精確、實時定位的要求。
【專利說明】
一種懸臂式掘進(jìn)機(jī)空間位姿實時檢測系統(tǒng)及方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明涉及一種礦用懸臂式掘進(jìn)機(jī)的空間位姿實時檢測系統(tǒng)及方法，屬采掘設(shè)備 應(yīng)用和機(jī)器視覺應(yīng)用領(lǐng)域。
【背景技術(shù)】
[0002] 懸臂式掘進(jìn)機(jī)是一種常見的采掘機(jī)械，被廣泛應(yīng)用于煤礦巷道和隧道的挖掘中。
[0003] 當(dāng)前，掘進(jìn)機(jī)的掘進(jìn)過程主要依靠激光標(biāo)靶導(dǎo)向以及人工目測實現(xiàn)，易受到現(xiàn)場 光線和人為等因素干擾，從而造成對巷道的超挖、欠挖現(xiàn)象，導(dǎo)致企業(yè)采煤效率低下甚至引 發(fā)安全問題。因此，實現(xiàn)掘進(jìn)機(jī)掘進(jìn)過程的自動化和智能化就顯得尤為迫切，而要解決掘進(jìn) 機(jī)的自動精確掘進(jìn)就必須完成對其機(jī)身空間位姿參數(shù)的自動實時檢測?；诹Ⅲw計算機(jī)視 覺的掘進(jìn)機(jī)機(jī)身位姿檢測技術(shù)，其技術(shù)實質(zhì)就是利用機(jī)器視覺技術(shù)代替人眼來檢測和分析 空間剛體的位姿，采用該技術(shù)方案具有硬件成本低、操作簡便、測量精度高等特點(diǎn)。
[0004] 已有發(fā)明專利如公開號為CN101819036A和公開號CN102878976A，均采用立體計算 機(jī)視覺的原理測量掘進(jìn)機(jī)的機(jī)身位姿參數(shù)，但由于此類發(fā)明專利建立模型時，采用點(diǎn)激光 源作為指向單元，故均存在激光指向單元發(fā)射的點(diǎn)光源易受遮擋，從而造成測量系統(tǒng)準(zhǔn)確 性差、不穩(wěn)定，甚至導(dǎo)致無法檢測出掘進(jìn)機(jī)位姿參數(shù)。所以，此類發(fā)明具有局限性，不能進(jìn)一 步推廣。

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

[0005] 本發(fā)明專利針對【背景技術(shù)】的不足，提出一種懸臂式掘進(jìn)機(jī)空間位姿實時檢測系統(tǒng) 及方法，可實時準(zhǔn)確地檢測出掘進(jìn)機(jī)機(jī)身的空間位姿參數(shù)，且具有抗干擾性強(qiáng)、穩(wěn)定性高等 特點(diǎn)。
[0006] 為了實現(xiàn)上述目的，本發(fā)明是通過以下技術(shù)方案實現(xiàn)的：
[0007] 本發(fā)明專利是一種懸臂式掘進(jìn)機(jī)空間位姿實時檢測系統(tǒng)及方法，可實時獲取掘進(jìn) 機(jī)的位姿參數(shù)，包括偏航角a、俯仰角0、橫滾角y、以及機(jī)身中心點(diǎn)D(d X，dy，dz)的坐標(biāo)，由 于在實際操作中車前距L(即dy)對于掘進(jìn)機(jī)的糾偏用處不大，故所述檢測系統(tǒng)及方法只計 算了掘進(jìn)機(jī)機(jī)身上固定一點(diǎn)得dx與dz的值。
[0008] 進(jìn)一步的，所述系統(tǒng)包括兩臺十字激光發(fā)射器、一臺工控計算機(jī)、兩臺網(wǎng)絡(luò)攝像 機(jī)、兩個激光標(biāo)靶。
[0009] 進(jìn)一步的，所述兩臺十字激光發(fā)射器為礦用防爆十字激光發(fā)射器，鉛直懸掛固定 于巷道頂板，分別向所述兩個激光標(biāo)靶發(fā)射十字激光信號。
[0010] 進(jìn)一步的，所述兩個激光標(biāo)靶分別固定在掘進(jìn)機(jī)機(jī)身水平面的左前方與右后方， 用于接收所述十字激光發(fā)射器發(fā)射的十字激光的投影。
[0011] 進(jìn)一步的，所述兩臺網(wǎng)絡(luò)攝像機(jī)為礦用防爆網(wǎng)絡(luò)攝像機(jī)，分別安裝于所述兩個激 光標(biāo)靶的正后方，用于采集掘進(jìn)機(jī)運(yùn)行時刻在所述激光標(biāo)靶的十字激光成像。
[0012] 進(jìn)一步的，所述工控計算機(jī)可以實時處理大量圖形化數(shù)據(jù)，通過與所述兩臺網(wǎng)絡(luò) 攝像機(jī)連接，分別提取所述網(wǎng)絡(luò)攝像機(jī)采集的左右激光標(biāo)靶上的成像并進(jìn)行圖像處理和特 征參數(shù)提取。
[0013] 進(jìn)一步的，經(jīng)所述工控計算機(jī)對所述網(wǎng)絡(luò)攝像機(jī)采集圖像進(jìn)行處理并提取特征參 數(shù)之后，從所述激光標(biāo)靶靶面上可解算出激光圖像的光學(xué)特征點(diǎn)的坐標(biāo)，包括所述十字激 光發(fā)射器發(fā)射出的十字激光和所述激光標(biāo)靶靶面交點(diǎn)的坐標(biāo)，所述十字激光發(fā)射器發(fā)射出 的十字激光在所述激光標(biāo)靶靶面投影的中心點(diǎn)坐標(biāo)。
[0014] 進(jìn)一步的，根據(jù)巷道坐標(biāo)系和機(jī)身坐標(biāo)系雙坐標(biāo)系模型和上述解算出的特征點(diǎn)的 坐標(biāo)，進(jìn)行位姿解算，最終得到掘進(jìn)機(jī)在巷道坐標(biāo)系下的空間位姿參數(shù)，即偏航角a、俯仰角 0、橫滾角y、左右偏距dx與上下偏距dz的值。
[0015] 與已有技術(shù)相比，本發(fā)明有益效果是：
[0016] 本發(fā)明將機(jī)器視覺技術(shù)應(yīng)用于懸臂式掘進(jìn)機(jī)機(jī)身位姿檢測系統(tǒng)，將硬件軟件化， 具有成本低、操作簡便、測量精度高等特點(diǎn)。此外，本發(fā)明還采用雙十字激光發(fā)射器，有效解 決了點(diǎn)光源易受遮擋造成的測量系統(tǒng)不穩(wěn)定、測量結(jié)果不精確的問題，具有抗干擾性強(qiáng)、成 本可控等特點(diǎn)，具備一定市場推廣價值。
【附圖說明】
[0017] 附圖1為本發(fā)明掘進(jìn)機(jī)與測量設(shè)備相互位置圖；
[0018] 附圖2為本發(fā)明巷道坐標(biāo)系與掘進(jìn)機(jī)機(jī)身坐標(biāo)系模型圖；
[0019] 附圖3為本發(fā)明掘進(jìn)機(jī)空間位姿檢測流程圖；
[0020] 附圖4為本發(fā)明雙坐標(biāo)系機(jī)身位姿數(shù)學(xué)模型向量與角度轉(zhuǎn)換示意圖；
[0021 ]附圖5為本發(fā)明激光標(biāo)靶成像特征點(diǎn)解算示意圖；
[0022] 附圖6為本發(fā)明掘進(jìn)機(jī)空間位姿解算示意圖；
[0023] 附圖7為本發(fā)明機(jī)身坐標(biāo)系與巷道坐標(biāo)系坐標(biāo)轉(zhuǎn)換示意圖。
【具體實施方式】
[0024] 為了能夠更清楚地理解本發(fā)明的上述目的、特征、優(yōu)點(diǎn)，下面結(jié)合附圖和具體實施 方式對本發(fā)明進(jìn)一步詳細(xì)闡述。
[0025]設(shè)備安裝及模型的建立：
[0026]參考附圖1所示，本發(fā)明專利是一種懸臂式掘進(jìn)機(jī)空間位姿實時檢測系統(tǒng)及方法， 包括兩臺十字激光發(fā)射器1、2,兩個激光標(biāo)靶4、6,兩臺網(wǎng)絡(luò)攝像機(jī)5、7和一臺工控計算機(jī)。 [0027]所述兩臺十字激光發(fā)射器1、2分別鉛直懸掛固定于巷道頂板，安裝時，保證兩束水 平激光重疊并平行于掘進(jìn)機(jī)預(yù)設(shè)行進(jìn)方向，所述兩臺十字激光發(fā)射器1、2發(fā)射的十字激光 平面分別與所述激光標(biāo)靶平面4、6垂直，所述兩臺激光發(fā)射器中心與巷道中軸線距離相等， 所述兩臺激光發(fā)射器間距為1;所述兩個激光標(biāo)靶4、6分別固定在掘進(jìn)機(jī)機(jī)身水平面的左前 方與右后方，安裝時保證所述兩個激光標(biāo)靶平行于掘進(jìn)機(jī)機(jī)身的鉛直面且兩束十字激光分 別在所述兩個激光標(biāo)靶上的初始成像位于中心位置;所述兩臺網(wǎng)絡(luò)攝像機(jī)5、7安裝于標(biāo)靶 的正后方，并調(diào)整相互之間距離，以保證成像的完整性。
[0028]參考附圖2和附圖4所示，建立巷道坐標(biāo)系和機(jī)身坐標(biāo)系雙坐標(biāo)系機(jī)身位姿數(shù)學(xué)模 型，所述模型要求巷道斷面坐標(biāo)系為〇hXhYhZh，初始位置時刻所述掘進(jìn)機(jī)機(jī)身中線與巷道中 線重合，所述巷道坐標(biāo)系原點(diǎn)Oh位于巷道斷面的幾何中心，Xh軸指向巷道右側(cè)，Yh軸與巷道 中軸線平行并指向掘進(jìn)機(jī)前進(jìn)方向，Zh軸垂直與上兩軸構(gòu)成右手關(guān)系并取向上為正方向。 [0029]所述機(jī)身坐標(biāo)系為ObXbYbZb建立在掘進(jìn)機(jī)機(jī)身上，原點(diǎn)0 b位于機(jī)身中軸線上，在初 始位置時Xb、Yb、Zb三軸分別與Xh、Yh、Zh三軸平行。
[0030] 所述模型可簡化為巷道坐標(biāo)系下長方體剛體位姿的確定，即機(jī)身坐標(biāo)系的Xb、Yb、 Zb在巷道坐標(biāo)系下的向量(郊、濟(jì)、蘇）以及剛體上任意一點(diǎn)在該坐標(biāo)系下的坐標(biāo)。
[0031] 其中，機(jī)身坐標(biāo)系下的機(jī)身三個平面的法向量與H y的轉(zhuǎn)換關(guān)系為：
[0033] 進(jìn)而可得，
[0035]掘進(jìn)機(jī)機(jī)身位姿檢測整體流程：
[0036]參考附圖3所示，首先使用所述工控計算機(jī)對所述兩臺網(wǎng)絡(luò)攝像機(jī)中提取的所述 兩個激光標(biāo)靶上的成像進(jìn)行取樣，可定時對采集到的視頻流取樣一幀圖像。
[0037]進(jìn)一步的，使用所述工控計算機(jī)處理圖像并提取特征參數(shù)，先用Retinex圖像增強(qiáng) 算法對圖像進(jìn)行自適應(yīng)亮度調(diào)節(jié)，由于采集到的圖像產(chǎn)生了較大的桶行畸變，故需對圖像 進(jìn)行畸變校正，再通過對色彩的處理過濾掉兩個靶面圖像中不需要的激光線，使用直線邊 緣檢測的方法并根據(jù)標(biāo)靶安裝的實際尺寸，解算出機(jī)身坐標(biāo)系下所述十字激光發(fā)射器發(fā)射 出的十字激光分別與所述激光成像標(biāo)靶靶面交點(diǎn)的坐標(biāo)及所述十字激光發(fā)射器發(fā)射出的 十字激光中心點(diǎn)在所述激光靶面成像的坐標(biāo)。
[0038]所述解算過程參考附圖4,以所述激光標(biāo)靶^:為例，分別在極坐標(biāo)下計算出直線 li、l2與四個參考點(diǎn)(Pi-P4)連線的交點(diǎn)A、B、E、F、M在圖像坐標(biāo)系下的坐標(biāo)，再按式（1)轉(zhuǎn)換 到機(jī)身坐標(biāo)系下：
[0040] 其中，（x，y)為五點(diǎn)在圖像坐標(biāo)系中的坐標(biāo)，h為Pi、P4間的實際距離，w為間的 實際距離，（X，Y，Z)為5點(diǎn)在機(jī)身坐標(biāo)系中的坐標(biāo)。
[0041] 進(jìn)一步的，根據(jù)上述兩個激光標(biāo)靶yi、丫：中10個點(diǎn)的坐標(biāo)，解算出巷道截面坐標(biāo) 系下掘進(jìn)機(jī)的偏航角、俯仰角、橫滾角以及機(jī)身上固定一點(diǎn)在巷道截面上的偏移量，即掘進(jìn) 機(jī)的空間位姿(a，0, y，dx，dz)。
[0042] 機(jī)身相對巷道坐標(biāo)系下的偏航角a、俯仰角0與橫滾角y解算過程參考附圖6,將所 述兩臺十字激光發(fā)射器產(chǎn)生的重疊水平面視為面a，左十字激光豎直面為扮，右十字激光豎 直面為此，右十字激光(a、fe)在靶面 yi上投影的兩條相交直線為AB、EF并交于點(diǎn)M，左十字 激光(a、fo)在靶面y 2上投影的兩條相交直線為CD、GH并交于點(diǎn)N。因面y :與面y 2平行，故 水平激光面a在所述激光標(biāo)靶靶面上形成的兩條直線AB與CD相互平行，通過上一步的圖像 處理與所述激光標(biāo)靶的實際尺寸與安裝位置可以得到71與^2上十個點(diǎn)的具體坐標(biāo)，從而 得到平面a在坐標(biāo)系ObXbYbZb下的法向量與平面方程。
[0043] 特殊情況下，向量屈|平行于向量^",此時掘進(jìn)機(jī)的俯仰角0為零，橫滾角Y為直 線CD與軸Xb間的夾角，偏航角a為g與初始位置時的她處間的夾角；若向量g不平行于 向量C,則涵x應(yīng)即為平面01的法向量瓦T,兩、.范分別于巷道截面坐標(biāo)系Xh、Zh的方向 相同，Yh的方向即為gxg (￥)，易求得，線段CD延長線上的一點(diǎn)N'滿足]即右 十字激光的中心軸線，從而將平面扮右移到平面此的位置，模擬了理想狀態(tài)下的十字激光投 影效果，為點(diǎn)坐標(biāo)的精確解算提供條件。
[0044] 參考附圖7,上述所得的瓦瓦|為機(jī)身坐標(biāo)系0bXbYbZb下的機(jī)身方向向量， 須再進(jìn)行坐標(biāo)轉(zhuǎn)換，得到巷道坐標(biāo)系OhXhYhZh下機(jī)身的方向向量。具體解算過程如下：對矩 陣(Nx，Ny，Nz )%矩陣變換，與轉(zhuǎn)換矩陣R相乘轉(zhuǎn)換為單位矩陣，即可得到0hXhYhZ h坐標(biāo)系下 三個軸的單位向量。
[0045] N ? R = E (4)
[0046] 向量在OhXhYhZh坐標(biāo)系下為單位矩陣E，對單位矩陣做相同變換得到 的矩陣，即為OhXhYhZh坐標(biāo)系下的機(jī)身位姿。
[0047] B = E ? R (5)
[0047] SPB = N-S 可得到
[0049] 再由（2)式即可得到機(jī)身相對巷道坐標(biāo)系下的偏航角a、俯仰角0與橫滾角Y。
[0050] 機(jī)身相對巷道坐標(biāo)系下的點(diǎn)坐標(biāo)解算過程如下：
[0051]進(jìn)一步的，為確定空間內(nèi)一剛體的位姿，只知道機(jī)體坐標(biāo)系三個軸的方向角并不 能精確定位，還需得到剛體上任意一點(diǎn)〇1在空間內(nèi)的具體坐標(biāo)，即（〇1\，〇^，〇12)。在掘進(jìn)機(jī) 定位系統(tǒng)中，掘進(jìn)機(jī)在巷道截面上左右、上下的偏移距離比沿巷道中心軸走過的距離更為 有效的幫助礦井工作人員發(fā)現(xiàn)掘進(jìn)機(jī)的行走偏差并及時糾偏。且由于所述檢測系統(tǒng)采用視 覺定位的方法，無法確切知曉掘進(jìn)機(jī)與巷道坐標(biāo)系坐標(biāo)原點(diǎn)間的軸向距離〇 ly，故所述檢測 系統(tǒng)針對點(diǎn)坐標(biāo)的解算只計算了（〇lx，〇lz)，即在巷道截面上Ch點(diǎn)相對于斷面中心點(diǎn)的偏移 距離。
[0052] 參考附圖6,直線MN'為右十字激光中心軸線，平行于巷道截面坐標(biāo)系的Yh軸。所述 檢測系統(tǒng)選擇〇b點(diǎn)為機(jī)身定位點(diǎn)，求點(diǎn)0b在面ZhOhXh上相對巷道中軸線的偏移，可先簡化為 求點(diǎn)0b在平行于ZhOhXh-平面上的投影相對MN'投影點(diǎn)的偏移，即在直線麗'上找一點(diǎn)0〇,滿 足5^)赫=〇6
[0053] 點(diǎn)0b在坐標(biāo)平面ZhOhXh上投影點(diǎn)的坐標(biāo)(dx，dz)計算方法如下，
[0056]本發(fā)明中涉及的未說明部分與現(xiàn)有技術(shù)相同或采用現(xiàn)有技術(shù)加以實現(xiàn)。
【主權(quán)項】
1. 一種懸臂式掘進(jìn)機(jī)空間位姿實時檢測系統(tǒng)及方法，其特征在于，包括:兩臺十字激光 發(fā)射器、一臺工控計算機(jī)、兩臺網(wǎng)絡(luò)攝像機(jī)、兩個激光標(biāo)靶，其中， 所述兩臺十字激光發(fā)射器為礦用防爆十字激光發(fā)射器，分別鉛直懸掛固定于巷道頂 板，發(fā)射十字激光束； 所述兩個激光標(biāo)靶為磨砂面半透明PC板，固定在掘進(jìn)機(jī)機(jī)身水平面板的左前方與右后 方，分別用于接收所述十字激光發(fā)射器發(fā)射的十字激光的投影； 所述兩臺網(wǎng)絡(luò)攝像機(jī)為礦用防爆網(wǎng)絡(luò)攝像機(jī)，分別安裝于所述兩個激光標(biāo)靶的正后 方，采集掘進(jìn)機(jī)運(yùn)行時刻在所述激光成像標(biāo)靶的十字激光成像； 所述工控計算機(jī)通過與所述兩臺網(wǎng)絡(luò)攝像機(jī)連接，分別提取所述網(wǎng)絡(luò)攝像機(jī)采集的左 右激光標(biāo)靶上的圖像，并進(jìn)行處理和特征參數(shù)提取，最終得到掘進(jìn)機(jī)在巷道坐標(biāo)系下的空 間位姿參數(shù)。2. 根據(jù)權(quán)利要求1所述的一種懸臂式掘進(jìn)機(jī)空間位姿實時檢測系統(tǒng)及方法，其特征在 于，所述兩臺十字激光器的安裝需保證兩束水平激光重疊并平行于掘進(jìn)機(jī)預(yù)設(shè)行進(jìn)方向， 所述兩臺十字激光發(fā)射器發(fā)射的十字激光平面分別與所述激光標(biāo)靶平面垂直，所述兩臺激 光發(fā)射器中心與巷道中軸線距離相等，所述兩臺激光發(fā)射器間距為1。3. 根據(jù)權(quán)利要求1所述的一種懸臂式掘進(jìn)機(jī)空間位姿實時檢測系統(tǒng)及方法，其特征在 于，所述兩個激光標(biāo)靶安裝時需保證平行于掘進(jìn)機(jī)機(jī)身的尾部平面且兩束十字激光分別在 所述兩個激光標(biāo)靶上的初始成像位于中心位置。4. 根據(jù)權(quán)利要求1所述的一種懸臂式掘進(jìn)機(jī)空間位姿實時檢測系統(tǒng)及方法，其特征在 于，所述兩臺網(wǎng)絡(luò)攝像機(jī)安裝時需保證分別位于所述激光標(biāo)靶的正后方，并調(diào)整相互之間 距離保證成像的完整性。5. 根據(jù)權(quán)利要求1所述的一種懸臂式掘進(jìn)機(jī)空間位姿實時檢測系統(tǒng)及方法，其特征在 于，所述工控計算機(jī)能夠即時處理各類數(shù)據(jù)，安裝在掘進(jìn)機(jī)內(nèi)部，與掘進(jìn)機(jī)剛性連接，通過 網(wǎng)絡(luò)連接線分別與所述兩臺網(wǎng)絡(luò)攝像機(jī)連接，獲取所述兩個激光標(biāo)靶的成像，并進(jìn)行圖像 處理和特征參數(shù)提取，最終計算出掘進(jìn)機(jī)在巷道坐標(biāo)系下的空間位姿參數(shù)。6. 根據(jù)權(quán)利要求1至5所述的一種懸臂式掘進(jìn)機(jī)空間位姿實時檢測系統(tǒng)及方法，其特征 在于，所述懸臂式掘進(jìn)機(jī)空間位姿實時檢測系統(tǒng)檢測方法為:使用兩臺固定在巷道上的所 述十字激光器向?qū)?yīng)的所述激光標(biāo)靶進(jìn)行投影，然后通過兩個固定在掘進(jìn)機(jī)機(jī)身上的所述 網(wǎng)絡(luò)攝像機(jī)實時采集掘進(jìn)機(jī)工作時所述激光標(biāo)靶上的成像，隨后使用Retinex圖像增強(qiáng)算 法、圖像畸變校正和直線檢測等圖像處理方法對采集到的視頻信號進(jìn)行處理，最終利用建 立好的位姿解算模型得到掘進(jìn)機(jī)行進(jìn)過程中的偏航角、俯仰角、橫滾角以及機(jī)身上固定一 點(diǎn)在巷道截面上的偏移量。
【文檔編號】G01C11/00GK106052645SQ201610141544
【公開日】2016年10月26日
【申請日】2016年3月11日 公開號201610141544.9, CN 106052645 A, CN 106052645A, CN 201610141544, CN-A-106052645, CN106052645 A, CN106052645A, CN201610141544, CN201610141544.9
【發(fā)明人】董海波, 劉停, 杜雨馨, 周玲玲, 童敏明, 梁良
【申請人】中國礦業(yè)大學(xué)