專利名稱：基于攝像頭和航姿儀的掘進(jìn)自動(dòng)導(dǎo)向系統(tǒng)及導(dǎo)向方法
技術(shù)領(lǐng)域：
本發(fā)明涉及掘進(jìn)機(jī)位姿測量技術(shù)領(lǐng)域，具體涉及一種基于攝像頭和航姿儀的掘進(jìn)自動(dòng)導(dǎo)向系統(tǒng)及導(dǎo)向方法。
背景技術(shù)：
目 前，我國的巷道掘進(jìn)導(dǎo)向技術(shù)一般采用激光指向儀，掘進(jìn)機(jī)駕駛員通過目視端面上的激光光斑控制掘進(jìn)機(jī)截割頭在斷面上截割運(yùn)行，由于測量工作只能在掘進(jìn)機(jī)停止截割的間隙中進(jìn)行，因此測量浪費(fèi)大量的時(shí)間，工作強(qiáng)度大，掘進(jìn)的質(zhì)量完全取決于駕駛員的熟練程度和經(jīng)驗(yàn)，測量結(jié)果的準(zhǔn)確性無法得到保證。為解決以上問題，國內(nèi)外的相關(guān)研究人員提出了很多的解決方案，公開號(hào)為CN101629807的中國專利《掘進(jìn)機(jī)機(jī)身位姿測量系統(tǒng)及其方法》提出了利用扇形激光束在掘進(jìn)機(jī)機(jī)身形成線性光斑，依靠帶有光敏元件的激光標(biāo)靶來產(chǎn)生電流信號(hào)，經(jīng)過處理后得出掘進(jìn)機(jī)的偏向角和偏向位移的方案。但是該方案在扇形激光束的獲取上很有難度，且扇形激光束的加工工藝和精度難以保證。公開號(hào)為CN101266134A的中國專利《懸臂機(jī)頭位姿的測量系統(tǒng)及其方法》提出了利用激光機(jī)動(dòng)全站儀發(fā)射激光到掘進(jìn)機(jī)，經(jīng)過棱鏡反射，再對(duì)反射回來的激光信號(hào)進(jìn)行接收，然后通過無線信號(hào)的傳輸?shù)竭_(dá)掘進(jìn)機(jī)內(nèi)的計(jì)算機(jī)，一次性地測量掘進(jìn)機(jī)的全部位姿參數(shù)的方案。該方案雖理論上可行，但具有操作復(fù)雜，對(duì)操作人員要求高，成本高等缺點(diǎn)，不易推廣使用。

發(fā)明內(nèi)容
本發(fā)明的目的是針對(duì)上述技術(shù)問題，提供一種基于攝像頭和航姿儀的掘進(jìn)自動(dòng)導(dǎo)向系統(tǒng)及導(dǎo)向方法，該導(dǎo)向系統(tǒng)和導(dǎo)線方法將CCD (Charge-coupled Device,電荷f禹合元件)攝像頭捕獲激光光斑與航姿儀相結(jié)合，完成掘進(jìn)機(jī)機(jī)身位姿參數(shù)(包括水平偏向位移和偏向角)的測量，并可根據(jù)偏向位移和偏向角對(duì)掘進(jìn)機(jī)進(jìn)行自動(dòng)糾偏，實(shí)現(xiàn)掘進(jìn)機(jī)的自動(dòng)導(dǎo)向。為實(shí)現(xiàn)此目的，本發(fā)明所設(shè)計(jì)的基于攝像頭和航姿儀的掘進(jìn)自動(dòng)導(dǎo)向系統(tǒng)，它包括設(shè)置在掘進(jìn)機(jī)機(jī)身上的掘進(jìn)機(jī)控制模塊，其特征在于:它還包括設(shè)置在掘進(jìn)機(jī)機(jī)身上的激光靶面、CXD攝像頭、航姿儀、數(shù)據(jù)采集模塊、中央處理器以及設(shè)置在掘進(jìn)巷道上的激光指向儀，所述激光指向儀位于掘進(jìn)機(jī)機(jī)身后方，其中，所述激光指向儀的激光發(fā)射頭對(duì)準(zhǔn)激光靶面的中心，所述CCD攝像頭投射在激光靶面上，所述CCD攝像頭的信號(hào)輸出端連接數(shù)據(jù)采集模塊，航姿儀的信號(hào)輸出端連接中央處理器，所述數(shù)據(jù)采集模塊的通信端連接中央處理器的通信端，中央處理器的控制信號(hào)輸出端連接掘進(jìn)機(jī)控制模塊的控制信號(hào)輸入端。本發(fā)明還包括分別設(shè)置在掘進(jìn)機(jī)機(jī)身四個(gè)角上的四個(gè)光電開關(guān)，所述每個(gè)光電開關(guān)的信號(hào)輸出端均連接中央處理器。所述數(shù)據(jù)采集模塊的通信端通過SPI (Serial Peripheral Interface,串行外圍設(shè)備接口)總線連接中央處理器的通信端；所述航姿儀的信號(hào)輸出端通過RS485總線連接中央處理器；所述中央處理器的控制信號(hào)輸出端通過CAN (Controller Area Network,控制器局域網(wǎng)絡(luò))總線連接掘進(jìn)機(jī)控制模塊的控制信號(hào)輸入端。所述每個(gè)光電開關(guān)的信號(hào)輸出端均通過RS485總線連接中央處理器。一種基于攝像頭和航姿儀的掘進(jìn)自動(dòng)導(dǎo)向系統(tǒng)的導(dǎo)向方法，其特征在于，它包括如下步驟:步驟1:打開激光指向儀，并控制激光指向儀射出與掘進(jìn)巷道平行的激光，該激光打在激光祀面的中心；步驟2:掘進(jìn)機(jī)開始工作并向前運(yùn)動(dòng)，CXD攝像頭捕獲激光靶面上的激光光斑，數(shù)據(jù)采集模塊采集到激光光斑實(shí)時(shí)位置數(shù)據(jù)，數(shù)據(jù)采集模塊將激光光斑實(shí)時(shí)位置數(shù)據(jù)傳輸給中央處理器，中央處理器運(yùn)算得到實(shí)時(shí)激光光斑中心點(diǎn)坐標(biāo)，通過實(shí)時(shí)激光光斑中心點(diǎn)坐標(biāo)得到掘進(jìn)機(jī)的實(shí)時(shí)水平偏向位移，所述水平偏向位移為在水平方向上偏離中心的距離；步驟3:在掘進(jìn)機(jī)向前運(yùn)動(dòng)的過程中，航姿儀實(shí)時(shí)采集掘進(jìn)機(jī)運(yùn)動(dòng)時(shí)的偏向角，并將掘進(jìn)機(jī)的實(shí)時(shí)偏向角數(shù)據(jù)輸送到中央處理器；步驟4:中央處理器將掘進(jìn)機(jī)的實(shí)時(shí)水平偏向位移和掘進(jìn)機(jī)的實(shí)時(shí)偏向角數(shù)據(jù)輸送給掘進(jìn)機(jī)控制模塊，掘進(jìn)機(jī)控制模塊根據(jù)上述掘進(jìn)機(jī)的實(shí)時(shí)水平偏向位移和掘進(jìn)機(jī)的實(shí)時(shí)偏向角數(shù)據(jù)發(fā)出控制指令使掘進(jìn)機(jī)進(jìn)行動(dòng)態(tài)糾偏，實(shí)現(xiàn)自動(dòng)導(dǎo)向。所述步驟4中，掘進(jìn)機(jī)控制模塊根據(jù)上述掘進(jìn)機(jī)的實(shí)時(shí)水平偏向位移和掘進(jìn)機(jī)的實(shí)時(shí)偏向角數(shù)據(jù)發(fā)出控制指令使掘 進(jìn)機(jī)進(jìn)行動(dòng)態(tài)糾偏的具體方法是:掘進(jìn)機(jī)控制模塊獲取掘進(jìn)機(jī)的水平偏向位移和偏向角后，首先掘進(jìn)機(jī)控制模塊調(diào)整掘進(jìn)機(jī)右輪的速度，從而調(diào)整掘進(jìn)機(jī)的偏向角，當(dāng)掘進(jìn)機(jī)的偏向角調(diào)整為O度時(shí)，再繼續(xù)通過掘進(jìn)機(jī)控制模塊調(diào)整掘進(jìn)機(jī)右輪的速度，從而調(diào)整掘進(jìn)機(jī)的水平偏向位移，這樣不斷重復(fù)，若掘進(jìn)機(jī)右偏則調(diào)慢掘進(jìn)機(jī)右輪速度，若掘進(jìn)機(jī)左偏則調(diào)快掘進(jìn)機(jī)右輪速度；上述掘進(jìn)機(jī)控制模塊控制掘進(jìn)機(jī)右輪的實(shí)時(shí)速度由下式得到:F = FHcX(XxX-AxXxf)⑴其中，V為掘進(jìn)機(jī)控制模塊控制掘進(jìn)機(jī)右輪的實(shí)時(shí)速度，V丨為上一時(shí)刻掘進(jìn)機(jī)右輪的行走速度，X為掘進(jìn)機(jī)的實(shí)際水平偏向位移，X丨為掘進(jìn)機(jī)上一時(shí)刻的水平偏向位移，K。為掘進(jìn)機(jī)車輪電機(jī)扭力調(diào)整系數(shù)，Kh是慣性抑制系數(shù)；通過上述調(diào)整，最終將掘進(jìn)機(jī)調(diào)整至中心軸線上，繼續(xù)行進(jìn)，實(shí)現(xiàn)掘進(jìn)機(jī)得自動(dòng)導(dǎo)向。所述步驟3和步驟4之間還包括步驟3.1:掘進(jìn)機(jī)機(jī)身四個(gè)角上的光電開關(guān)向四周發(fā)出光信號(hào)，光信號(hào)碰到掘進(jìn)巷道后被反射回對(duì)應(yīng)的光電開關(guān)，每個(gè)光電開關(guān)以此得到光電開關(guān)與掘進(jìn)巷道之間的距離，每個(gè)光電開關(guān)將該距離信息與光電開關(guān)上預(yù)先設(shè)好的閾值進(jìn)行比較，并將比較結(jié)果傳輸給中央處理器，當(dāng)有一個(gè)光電開關(guān)與掘進(jìn)巷道之間的距離小于該閾值時(shí)，中央處理器控制掘進(jìn)機(jī)控制模塊改變運(yùn)行軌跡，以使每個(gè)光電開關(guān)與掘進(jìn)巷道之間的距離保持在大于該閾值。所述步驟4的公式I中當(dāng)掘進(jìn)機(jī)的水平偏向位移左偏時(shí)，如果掘進(jìn)機(jī)的運(yùn)動(dòng)速度增加，則K。= 0.2 2 ,Kh = 0.9 ;如果掘進(jìn)機(jī)的運(yùn)動(dòng)速度減小，則K。= 2.2 ,Kh = 0.7 ；當(dāng)掘進(jìn)機(jī)的水平偏向位移右偏時(shí)，如果掘進(jìn)機(jī)的運(yùn)動(dòng)速度增加，則K。= 2.2，Kh = 0.7 ；如果掘進(jìn)機(jī)的運(yùn)動(dòng)速度減小，則K。= 0.2 2，Kh = 0.9。
所述步驟3.1中的閾值為2(T40cm。與現(xiàn)有技術(shù)相比，本發(fā)明具有如下優(yōu)點(diǎn):I)本發(fā)明使用激光指向儀，CCD攝像頭，航姿儀，光電開關(guān)、數(shù)據(jù)采集模塊和中央處理器，完成掘進(jìn)機(jī)位姿測量和自動(dòng)導(dǎo)向，可獲取掘進(jìn)機(jī)相對(duì)預(yù)定軌跡的偏向角和偏向位移。本系統(tǒng)的測量精度可達(dá)到0.5°以內(nèi)，偏離位移精度在5cm以內(nèi)。2)本發(fā)明將CCD攝像頭捕獲的激光光斑與航姿儀相結(jié)合，完成掘進(jìn)機(jī)機(jī)身位姿參數(shù)(包括水平偏向位移和偏向角)的測量，并可根據(jù)偏向位移和偏向角對(duì)掘進(jìn)機(jī)進(jìn)行自動(dòng)糾偏，實(shí)現(xiàn)掘進(jìn)機(jī)的自動(dòng)導(dǎo)向。3)本發(fā)明使用光電開關(guān)確保掘進(jìn)機(jī)不與巷道發(fā)生碰撞，保證了整個(gè)系統(tǒng)安全、正常地運(yùn)行。4)本發(fā)明相對(duì)背景技術(shù)中記載的兩個(gè)技術(shù)方案，具有導(dǎo)向精度高、操作簡單、成本低、易于實(shí)施便于推廣的優(yōu)點(diǎn)。


圖1為本發(fā)明的結(jié)構(gòu)示意圖；圖2為本發(fā)明的結(jié)構(gòu)框圖；圖3為掘進(jìn)機(jī)的運(yùn)行調(diào)整軌跡圖；圖4為本發(fā)明光電開關(guān)分布示意圖。其中，I一掘進(jìn)機(jī)機(jī)身、2—掘進(jìn)機(jī)控`制模塊、3—激光祀面、4一CO)攝像頭、5—航姿儀、6—數(shù)據(jù)采集模塊、7—中央處理器、8—掘進(jìn)巷道、9 一激光指向儀、10—光電開關(guān)。
具體實(shí)施例方式以下結(jié)合附圖和實(shí)施例對(duì)本發(fā)明作進(jìn)一步的詳細(xì)說明:本發(fā)明的基于攝像頭和航姿儀的掘進(jìn)自動(dòng)導(dǎo)向系統(tǒng)，如圖1和圖2所示，它包括設(shè)置在掘進(jìn)機(jī)機(jī)身I上的掘進(jìn)機(jī)控制模塊2、設(shè)置在掘進(jìn)機(jī)機(jī)身I上的激光靶面3、CXD攝像頭4、航姿儀5、數(shù)據(jù)采集模塊6、中央處理器7，設(shè)置在掘進(jìn)巷道8上的激光指向儀9，激光指向儀9位于掘進(jìn)機(jī)機(jī)身I后方，其中，激光指向儀9的激光發(fā)射頭對(duì)準(zhǔn)激光靶面3的中心，CCD攝像頭4投射在激光靶面3上，CCD攝像頭4的信號(hào)輸出端連接數(shù)據(jù)采集模塊6，航姿儀5的信號(hào)輸出端連接中央處理器7，數(shù)據(jù)采集模塊6的通信端連接中央處理器7的通信端，中央處理器7的控制信號(hào)輸出端連接掘進(jìn)機(jī)控制模塊2的控制信號(hào)輸入端。上述技術(shù)方案中，它還包括分別設(shè)置在掘進(jìn)機(jī)機(jī)身I四個(gè)角上的四個(gè)光電開關(guān)10，如圖4所示，每個(gè)光電開關(guān)10的信號(hào)輸出端均連接中央處理器7。上述技術(shù)方案中，數(shù)據(jù)采集模塊6的通信端通過SPI總線連接中央處理器7的通信端；航姿儀5的信號(hào)輸出端通過RS485總線連接中央處理器7 ;中央處理器7的控制信號(hào)輸出端通過CAN總線連接掘進(jìn)機(jī)控制模塊2的控制信號(hào)輸入端。上述技術(shù)方案中，每個(gè)光電開關(guān)10的信號(hào)輸出端均通過RS485總線連接中央處理器7。上述技術(shù)方案中，數(shù)據(jù)采集模塊6為ARM9，所述ARM9采用AT91SAM9G20。航姿儀5采用重慶冰刃科技有限公司的Avatar II型航姿儀，該航姿儀5以磁力傳感器、重力加速度計(jì)以及陀螺儀為架構(gòu)核心，該航姿儀5通過采集運(yùn)動(dòng)傳感器的數(shù)據(jù)，融合卡爾曼濾波，輸出實(shí)時(shí)掘進(jìn)機(jī)機(jī)身的姿態(tài)角數(shù)據(jù)。CXD攝像頭4采用工業(yè)級(jí)的圖像傳感器0V7725。所述中央處理器 使用C8051F040。光電開關(guān)10選用歐姆龍公司的E3G-L77漫反射型光電開關(guān)，其感應(yīng)距離最遠(yuǎn)可達(dá)2m，且能適應(yīng)黑色和粗糙的表面。上述技術(shù)方案中，當(dāng)掘進(jìn)機(jī)在狹窄的掘進(jìn)巷道8內(nèi)進(jìn)行姿態(tài)調(diào)整時(shí)，容易與掘進(jìn)巷道8巖壁發(fā)生碰撞，使用光電開關(guān)10來提醒和強(qiáng)制避免掘進(jìn)機(jī)與掘進(jìn)巷道8發(fā)生碰撞。在掘進(jìn)機(jī)機(jī)身I的四個(gè)角上安裝光電開關(guān)10作為應(yīng)急響應(yīng)，當(dāng)四個(gè)光電開關(guān)10當(dāng)中的某一個(gè)與巖壁的距離小于安全閾值時(shí)，就讓光電開關(guān)10作為主導(dǎo)，控制掘進(jìn)機(jī)調(diào)整，讓掘進(jìn)機(jī)重新回到掘進(jìn)巷道8中間。本發(fā)明工作時(shí):掘進(jìn)機(jī)根據(jù)激光指向儀指向方向直行，如偏離激光指向方向2CM，就控制掘進(jìn)機(jī)以半圓弧軌跡從偏向的反方向回到激光的中心軸線上，掘進(jìn)機(jī)重新按照激光指向方向前進(jìn)。將掘進(jìn)機(jī)調(diào)整到中心軸線上的方法為:當(dāng)攝像頭檢測到掘進(jìn)機(jī)的水平偏向位移大于2cm時(shí)，通過調(diào)整掘進(jìn)機(jī)左右輪的位移差來減小掘進(jìn)機(jī)的水平偏向位移，當(dāng)調(diào)整到掘進(jìn)機(jī)的水平偏向位移小于2CM時(shí)，停止調(diào)整位移，通過原地調(diào)整角度，角度調(diào)整至初始狀態(tài)。圖3中的軌跡A-B-C-D是掘進(jìn)機(jī)的運(yùn)行軌跡。A-B段時(shí)，掘進(jìn)機(jī)的右輪速度大于左輪速度，目的是讓掘進(jìn)機(jī)做轉(zhuǎn)向動(dòng)作。且隨著水平偏向位移(即與激光方向的位移)的增大，轉(zhuǎn)向力度越大；B-C-D段時(shí)，左輪速度逐漸增大，最后掘進(jìn)機(jī)處于右輪速度小于左輪速度的狀態(tài)，目的是讓水平偏向位移為O (即掘進(jìn)機(jī)回到既定路線上)時(shí)掘進(jìn)機(jī)運(yùn)動(dòng)方向與激光方向夾角處在一個(gè)可控狀態(tài)(運(yùn)動(dòng)方向的夾角與激光方向夾角不宜過大)。具體的計(jì)算公式如⑵和⑶所示。
權(quán)利要求
1.一種基于攝像頭和航姿儀的掘進(jìn)自動(dòng)導(dǎo)向系統(tǒng)，它包括設(shè)置在掘進(jìn)機(jī)機(jī)身(I)上的掘進(jìn)機(jī)控制模塊(2)，其特征在于:它還包括設(shè)置在掘進(jìn)機(jī)機(jī)身(I)上的激光靶面(3)、CXD攝像頭(4)、航姿儀(5)、數(shù)據(jù)采集模塊(6)、中央處理器(7)以及設(shè)置在掘進(jìn)巷道(8)上的激光指向儀(9)，所述激光指向儀(9)位于掘進(jìn)機(jī)機(jī)身(I)后方，其中，所述激光指向儀(9)的激光發(fā)射頭對(duì)準(zhǔn)激光靶面(3)的中心，所述CCD攝像頭(4)投射在激光靶面(3)上，所述CCD攝像頭(4)的信號(hào)輸出端連接數(shù)據(jù)采集模塊(6)，航姿儀(5)的信號(hào)輸出端連接中央處理器(7),所述數(shù)據(jù)采集模塊(6)的通信端連接中央處理器(7)的通信端，中央處理器(7)的控制信號(hào)輸出端連接掘進(jìn)機(jī)控制模塊(2 )的控制信號(hào)輸入端。
2.根據(jù)權(quán)利要求1所述的基于攝像頭和航姿儀的掘進(jìn)自動(dòng)導(dǎo)向系統(tǒng)，其特征在于:它還包括分別設(shè)置在掘進(jìn)機(jī)機(jī)身(I)四個(gè)角上的四個(gè)光電開關(guān)(10)，所述每個(gè)光電開關(guān)(10)的信號(hào)輸出端均連接中央處理器(7 )。
3.根據(jù)權(quán)利要求1所述的基于攝像頭和航姿儀的掘進(jìn)自動(dòng)導(dǎo)向系統(tǒng)，其特征在于:所述數(shù)據(jù)采集模塊(6)的通信端通過SPI總線連接中央處理器(7)的通信端；所述航姿儀(5)的信號(hào)輸出端通過RS485總線連接中央處理器(7);所述中央處理器(7)的控制信號(hào)輸出端通過CAN總線連接掘進(jìn)機(jī)控制模塊(2)的控制信號(hào)輸入端。
4.根據(jù)權(quán)利要求2所述的基于攝像頭和航姿儀的掘進(jìn)自動(dòng)導(dǎo)向系統(tǒng)，其特征在于:所述每個(gè)光電開關(guān)(10)的信號(hào)輸出端均通過RS485總線連接中央處理器(7)。
5.根據(jù)權(quán)利要求1或2所述基于攝像頭和航姿儀的掘進(jìn)自動(dòng)導(dǎo)向系統(tǒng)，所述系統(tǒng)的導(dǎo)向方法包括如下步驟: 步驟1:打開激光指向儀(9)，并控制激光指向儀(9)射出與掘進(jìn)巷道(8)平行的激光，該激光打在激光靶面(3)的中心； 步驟2:掘進(jìn)機(jī)開始工作并向前運(yùn) 動(dòng)，CXD攝像頭(4)捕獲激光靶面(3)上的激光光斑，數(shù)據(jù)采集模塊(6)采集到激光光斑實(shí)時(shí)位置數(shù)據(jù)，數(shù)據(jù)采集模塊(6)將激光光斑實(shí)時(shí)位置數(shù)據(jù)傳輸給中央處理器(7)，中央處理器(7)運(yùn)算得到實(shí)時(shí)激光光斑中心點(diǎn)坐標(biāo)，通過實(shí)時(shí)激光光斑中心點(diǎn)坐標(biāo)得到掘進(jìn)機(jī)的實(shí)時(shí)水平偏向位移，所述水平偏向位移為在水平方向上偏離中心的距離； 步驟3:在掘進(jìn)機(jī)向前運(yùn)動(dòng)的過程中，航姿儀實(shí)時(shí)采集掘進(jìn)機(jī)運(yùn)動(dòng)時(shí)的偏向角，并將掘進(jìn)機(jī)的實(shí)時(shí)偏向角數(shù)據(jù)輸送到中央處理器(7)； 步驟4:中央處理器(7)將掘進(jìn)機(jī)的實(shí)時(shí)水平偏向位移和掘進(jìn)機(jī)的實(shí)時(shí)偏向角數(shù)據(jù)輸送給掘進(jìn)機(jī)控制模塊(2)，掘進(jìn)機(jī)控制模塊(2)根據(jù)上述掘進(jìn)機(jī)的實(shí)時(shí)水平偏向位移和掘進(jìn)機(jī)的實(shí)時(shí)偏向角數(shù)據(jù)發(fā)出控制指令使掘進(jìn)機(jī)進(jìn)行動(dòng)態(tài)糾偏，實(shí)現(xiàn)自動(dòng)導(dǎo)向。
6.根據(jù)權(quán)利要求5所述的導(dǎo)向方法，其特征在于:所述步驟4中，掘進(jìn)機(jī)控制模塊(2)根據(jù)上述掘進(jìn)機(jī)的實(shí)時(shí)水平偏向位移和掘進(jìn)機(jī)的實(shí)時(shí)偏向角數(shù)據(jù)發(fā)出控制指令使掘進(jìn)機(jī)進(jìn)行動(dòng)態(tài)糾偏的具體方法是:掘進(jìn)機(jī)控制模塊(2)獲取掘進(jìn)機(jī)的水平偏向位移和偏向角后，首先掘進(jìn)機(jī)控制模塊(2)調(diào)整掘進(jìn)機(jī)右輪的速度，從而調(diào)整掘進(jìn)機(jī)的偏向角，當(dāng)掘進(jìn)機(jī)的偏向角調(diào)整為O度時(shí)，再繼續(xù)通過掘進(jìn)機(jī)控制模塊(2)調(diào)整掘進(jìn)機(jī)右輪的速度，從而調(diào)整掘進(jìn)機(jī)的水平偏向位移，這樣不斷重復(fù)，若掘進(jìn)機(jī)右偏則調(diào)慢掘進(jìn)機(jī)右輪速度，若掘進(jìn)機(jī)左偏則調(diào)快掘進(jìn)機(jī)右輪速度； 上述掘進(jìn)機(jī)控制模塊(2)控制掘進(jìn)機(jī)右輪的實(shí)時(shí)速度由下式得到:
7.根據(jù)權(quán)利要求5所述的導(dǎo)向方法，其特征在于:所述步驟3和步驟4之間還包括步驟3.1:掘進(jìn)機(jī)機(jī)身(I)四個(gè)角上的光電開關(guān)(10)向四周發(fā)出光信號(hào)，光信號(hào)碰到掘進(jìn)巷道(8)后被反射回對(duì)應(yīng)的光電開關(guān)(10)，每個(gè)光電開關(guān)(10)以此得到光電開關(guān)(10)與掘進(jìn)巷道(8)之間的距離，每個(gè)光電開關(guān)(10)將該距離信息與光電開關(guān)(10)上預(yù)先設(shè)好的閾值進(jìn)行比較，并將比較結(jié)果傳輸給中央處理器(7)，當(dāng)有一個(gè)光電開關(guān)(10)與掘進(jìn)巷道(8)之間的距離小于該閾值時(shí)，中央處理器(7)控制掘進(jìn)機(jī)控制模塊(2)改變運(yùn)行軌跡，以使每個(gè)光電開關(guān)(10)與掘進(jìn)巷道(8)之間的距離保持在大于該閾值。
8.根據(jù)權(quán)利要求6所述的導(dǎo)向方法，其特征在于:所述步驟4的公式I中當(dāng)掘進(jìn)機(jī)的水平偏向位移左偏時(shí)，如果掘進(jìn)機(jī)的運(yùn)動(dòng)速度增加，則K。= 0.2 2 ,Kh = 0.9 ;如果掘進(jìn)機(jī)的運(yùn)動(dòng)速度減小，則K。= 2.2,Kh= 0.7 ;當(dāng)掘進(jìn)機(jī)的水平偏向位移右偏時(shí)，如果掘進(jìn)機(jī)的運(yùn)動(dòng)速度增加，則K。= 2.2，Kh = 0.7 ;如果掘進(jìn)機(jī)的運(yùn)動(dòng)速度減小，則K。= 0.2 2，Kh = 0.9。
9.根據(jù)權(quán)利要求7所述的導(dǎo)向方法，其特征在于:所述步驟3.1中的閾值為2(T40cm。
全文摘要
本發(fā)明公開了一種基于攝像頭和航姿儀的掘進(jìn)自動(dòng)導(dǎo)向系統(tǒng)，它包括設(shè)置在掘進(jìn)機(jī)機(jī)身上的掘進(jìn)機(jī)控制模塊、設(shè)置在掘進(jìn)機(jī)機(jī)身上的激光靶面、攝像頭、航姿儀、數(shù)據(jù)采集模塊、中央處理器，設(shè)置在掘進(jìn)巷道上的激光指向儀，激光指向儀位于掘進(jìn)機(jī)機(jī)身后方，其中，激光指向儀的激光發(fā)射頭對(duì)準(zhǔn)激光靶面的中心，攝像頭投射在激光靶面上，攝像頭的信號(hào)輸出端連接數(shù)據(jù)采集模塊，航姿儀的信號(hào)輸出端連接中央處理器，數(shù)據(jù)采集模塊的通信端連接中央處理器的通信端，中央處理器的控制信號(hào)輸出端連接掘進(jìn)機(jī)控制模塊的控制信號(hào)輸入端。本發(fā)明具有導(dǎo)向精度高、操作簡單、成本低、易于實(shí)施便于推廣的優(yōu)點(diǎn)。
文檔編號(hào)G01B11/02GK103234513SQ20131013698
公開日2013年8月7日 申請(qǐng)日期2013年4月18日 優(yōu)先權(quán)日2013年4月18日
發(fā)明者劉新華, 強(qiáng)永龍, 劉果, 李依隆, 曾嬋, 許銀帆, 賈文卓, 張敬一 申請(qǐng)人:武漢理工大學(xué)