本發(fā)明屬于煤礦裝備自動(dòng)化技術(shù)領(lǐng)域，具體涉及一種四點(diǎn)式掘進(jìn)機(jī)自動(dòng)定位定向方法。

背景技術(shù)：

煤礦井下掘進(jìn)機(jī)工作面環(huán)境惡劣，又是事故高發(fā)區(qū)域，因此掘進(jìn)工作面的自動(dòng)化，少人無人化是煤礦的發(fā)展趨勢(shì)和迫切需求，掘進(jìn)機(jī)的自動(dòng)定位定向技術(shù)是實(shí)現(xiàn)掘進(jìn)工作面自動(dòng)化，少人無人化的關(guān)鍵技術(shù)。

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

本發(fā)明為解決掘進(jìn)機(jī)在井下巷道中作業(yè)時(shí)能實(shí)現(xiàn)自動(dòng)定位定向的技術(shù)問題，提供了一種四點(diǎn)式掘進(jìn)機(jī)自動(dòng)定位定向方法。

本發(fā)明采用的技術(shù)方案如下：

一種四點(diǎn)式掘進(jìn)機(jī)自動(dòng)定位定向方法，基于機(jī)器視覺技術(shù)的四點(diǎn)法獲得掘進(jìn)機(jī)機(jī)身空間位置和姿態(tài)參數(shù)；具體方法為：在掘進(jìn)機(jī)機(jī)身上表面選取四個(gè)特征點(diǎn)，將視覺定位定向儀安裝在掘進(jìn)機(jī)后方巷道頂板上，利用視覺定位定向儀中的工業(yè)智能攝像機(jī)對(duì)在掘進(jìn)機(jī)上選定的四個(gè)特征點(diǎn)成像，分析特征點(diǎn)圖像與已知的四個(gè)特征點(diǎn)之間的相對(duì)位置關(guān)系，計(jì)算得到掘進(jìn)機(jī)在工業(yè)智能攝像機(jī)坐標(biāo)系中的空間位置及空間姿態(tài)的偏轉(zhuǎn)角、橫滾角、俯仰角、機(jī)身的橫向偏移量、機(jī)身的縱向偏移量和機(jī)身到工業(yè)智能攝像機(jī)的距離六個(gè)參數(shù)，通過標(biāo)定工業(yè)智能攝像機(jī)與視覺定位定向儀上的激光器之間的相對(duì)位置關(guān)系，將掘進(jìn)機(jī)在工業(yè)智能攝像機(jī)坐標(biāo)系中的空間位置和姿態(tài)參數(shù)轉(zhuǎn)換到代表巷道設(shè)計(jì)基準(zhǔn)下的掘進(jìn)機(jī)空間位置和姿態(tài)參數(shù)。

所述四個(gè)特征點(diǎn)取在掘進(jìn)機(jī)機(jī)身的上表面，呈“口”字形分布，且四點(diǎn)相對(duì)于掘進(jìn)機(jī)機(jī)身縱向軸線對(duì)稱。

所述掘進(jìn)機(jī)在工業(yè)智能攝像機(jī)坐標(biāo)系中的偏轉(zhuǎn)角、橫滾角、俯仰角、機(jī)身的橫向偏移量、機(jī)身的縱向偏移量和機(jī)身到工業(yè)智能攝像機(jī)的距離的計(jì)算方法為：

機(jī)身與工業(yè)智能攝像機(jī)之間距離根據(jù)

計(jì)算出，各特征點(diǎn)A、B、C、D 在工業(yè)智能攝像機(jī)坐標(biāo)系中的橫向縱向二維空間坐標(biāo)根據(jù)

計(jì)算出，AB 和CD 的中點(diǎn)M、N 的橫向縱向二維空間坐標(biāo)根據(jù)

計(jì)算出，將MN 投影在水平面上，則MN，即掘進(jìn)機(jī)機(jī)身中軸線在工業(yè)智能攝像機(jī)坐標(biāo)系中的偏轉(zhuǎn)角根據(jù)

計(jì)算出；將MN 投影在鉛垂面上，則機(jī)身俯仰角根據(jù)

計(jì)算出；A、B 的像點(diǎn)在工業(yè)智能攝像機(jī)坐標(biāo)系中與其橫軸的夾角即為掘進(jìn)機(jī)機(jī)身的橫滾角；

以MN 的中點(diǎn)為機(jī)身中心，則機(jī)身的縱向偏移、橫向偏移、與工業(yè)智能攝像機(jī)之間的距離即掘進(jìn)機(jī)機(jī)身的空間位置坐標(biāo)(x, y, z)根據(jù)

計(jì)算出；

其中：A、B、C、D 為選定的特征點(diǎn)，、、、分別為A、B、C、D的像，AB、CD為機(jī)身特征點(diǎn)間距，MN為AB、CD的中點(diǎn)連線，f為工業(yè)智能攝像機(jī)鏡頭焦距。

本發(fā)明的有益效果：本發(fā)明提出的一種基于機(jī)器視覺技術(shù)的四點(diǎn)式掘進(jìn)機(jī)自動(dòng)定位定向方法，以掘進(jìn)機(jī)機(jī)身四個(gè)特定位置作為特征點(diǎn)，由工業(yè)智能攝像機(jī)獲取其圖像，從中解算掘進(jìn)機(jī)機(jī)身相對(duì)于攝像機(jī)的偏航角、橫滾角、俯仰角以及機(jī)身的橫向偏移量、縱向偏移量和機(jī)身到工業(yè)智能攝像機(jī)的距離等參數(shù)，實(shí)現(xiàn)掘進(jìn)機(jī)自動(dòng)定位定向，具有自動(dòng)化程度高，結(jié)構(gòu)簡(jiǎn)單，姿態(tài)信息完整、精度高等優(yōu)點(diǎn)。

附圖說明

圖1為視覺定位定向儀的結(jié)構(gòu)示意圖；

圖2為成像幾何模型；

圖3為特征點(diǎn)圖像；

圖4為特征點(diǎn)設(shè)置示意圖；

圖5為視覺定位定向儀方向調(diào)定方法示意圖；

圖中：1-工業(yè)智能攝像機(jī)，2-無線數(shù)傳系統(tǒng)，3-電源模塊，4-激光器，5-視覺定位定向儀。

具體實(shí)施方式

一種四點(diǎn)式掘進(jìn)機(jī)自動(dòng)定位定向方法，基于機(jī)器視覺技術(shù)的四點(diǎn)法獲得掘進(jìn)機(jī)機(jī)身空間位置和姿態(tài)參數(shù)；具體方法為：在掘進(jìn)機(jī)機(jī)身上表面選取四個(gè)特征點(diǎn)，將視覺定位定向儀安裝在掘進(jìn)機(jī)后方巷道頂板上，利用視覺定位定向儀中的工業(yè)智能攝像機(jī)對(duì)在掘進(jìn)機(jī)上選定的四個(gè)特征點(diǎn)成像，分析特征點(diǎn)圖像與已知的四個(gè)特征點(diǎn)之間的相對(duì)位置關(guān)系，計(jì)算得到掘進(jìn)機(jī)在工業(yè)智能攝像機(jī)坐標(biāo)系中的空間位置及空間姿態(tài)的偏轉(zhuǎn)角、橫滾角、俯仰角、機(jī)身的橫向偏移量、機(jī)身的縱向偏移量和機(jī)身到工業(yè)智能攝像機(jī)的距離六個(gè)參數(shù)，通過標(biāo)定工業(yè)智能攝像機(jī)與視覺定位定向儀上的激光器之間的相對(duì)位置關(guān)系，將掘進(jìn)機(jī)在工業(yè)智能攝像機(jī)坐標(biāo)系中的空間位置和姿態(tài)參數(shù)轉(zhuǎn)換到代表巷道設(shè)計(jì)基準(zhǔn)下的掘進(jìn)機(jī)空間位置和姿態(tài)參數(shù)。

所述四個(gè)特征點(diǎn)取在掘進(jìn)機(jī)機(jī)身的上表面，呈“口”字形分布，范圍盡可能大，且四點(diǎn)相對(duì)于掘進(jìn)機(jī)機(jī)身縱向軸線對(duì)稱。四個(gè)特征點(diǎn)與掘進(jìn)機(jī)后方巷道頂板上設(shè)置的視覺定位定向儀之間應(yīng)具有良好的通視性。

如圖1 所示，視覺定位定向儀由工業(yè)智能攝像機(jī)、無線數(shù)傳系統(tǒng)與激光器組合而成。激光器用于輔助調(diào)整工業(yè)智能攝像機(jī)光軸的方向，使其與巷道的設(shè)計(jì)方向一致；工業(yè)智能攝像機(jī)是一種具備圖像處理平臺(tái)并包含專用圖像處理軟件的工業(yè)攝像機(jī)，用于采集包含掘進(jìn)

機(jī)機(jī)身上四個(gè)特征點(diǎn)的場(chǎng)景圖像，并完成實(shí)時(shí)的圖像處理、目標(biāo)識(shí)別和掘進(jìn)機(jī)空間位置和空間姿態(tài)解算；通過無線數(shù)傳系統(tǒng)，將解算結(jié)果無線傳輸至上位機(jī)。

結(jié)構(gòu)上，視覺定位定向儀內(nèi)部，激光器與工業(yè)智能攝像機(jī)相連接并具有相對(duì)俯仰角和相對(duì)偏轉(zhuǎn)角調(diào)整功能，通過專用的標(biāo)定裝置調(diào)整二者間的俯仰角和偏轉(zhuǎn)角使其盡可能小并測(cè)定其數(shù)值作為系統(tǒng)修正值。視覺定位定向儀外部具有懸掛機(jī)構(gòu)，用于將視覺定位定向儀通過錨桿懸掛在巷道頂板上，視覺定位定向儀前方具有玻璃窗口，便于工業(yè)智能攝像機(jī)透過窗口采集圖像以及激光器投射出激光束。此外，視覺定位定向儀外部還具有俯仰角、偏轉(zhuǎn)角以及橫向偏移調(diào)整機(jī)構(gòu)，用于在視覺定位定向儀懸掛完成后精密調(diào)整，使其指向與巷道設(shè)計(jì)方向

一致。

本發(fā)明具體的實(shí)施及計(jì)算方法為:

1. 在掘進(jìn)機(jī)機(jī)身上設(shè)置特征點(diǎn)

如圖4所示，A、B、C、D 為選定的特征點(diǎn)，其中AB 和CD 均垂直于機(jī)身中軸線MN，AB、CD 及其MN的間距均人工測(cè)出。

2. 安裝視覺定位定向儀并利用激光器調(diào)定其指向

如圖5 所示，按照目前通行的所有煤礦巷道定位的方法，在巷道設(shè)計(jì)方向上懸掛兩條重錘線，然后在其后方大致一致的方向上的巷道頂板上打入錨桿，安裝視覺定位定向儀，利用視覺定位定向儀的三自由度調(diào)整機(jī)構(gòu)調(diào)整視覺定位定向儀的姿態(tài)和位置，使其激光束穿過兩個(gè)重錘線。

3. 工業(yè)智能攝像機(jī)采集機(jī)身特征點(diǎn)的圖像并進(jìn)行適當(dāng)?shù)膱D像處理、目標(biāo)識(shí)別和位姿參數(shù)解算。

4. 對(duì)計(jì)算中合理假設(shè)所帶來的計(jì)算誤差進(jìn)行修正。

5. 通過視覺定位定向儀中的無線數(shù)傳系統(tǒng)將工業(yè)智能攝像機(jī)解算出的機(jī)身空間位姿參數(shù)發(fā)出到上位機(jī)。

6. 上述3、4、5 步驟在視覺定位定向儀上電后自動(dòng)完成。

其中，掘進(jìn)機(jī)在工業(yè)智能攝像機(jī)坐標(biāo)系中的偏轉(zhuǎn)角、橫滾角、俯仰角、機(jī)身的橫向偏移量、機(jī)身的縱向偏移量和機(jī)身到工業(yè)智能攝像機(jī)的距離的具體計(jì)算方法為：

如圖2 所示，以AB 和CD 及其中點(diǎn)連線MN 組成的結(jié)構(gòu)表示掘進(jìn)機(jī)機(jī)身在水平面內(nèi)的投影，成像平面L 和焦點(diǎn)O 構(gòu)成攝像機(jī)成像幾何模型，X 軸表示掘進(jìn)機(jī)機(jī)身橫向偏移方向，Z 軸表示掘進(jìn)機(jī)機(jī)身前進(jìn)方向，與X 軸、Z 軸正交的Y 軸表示掘進(jìn)機(jī)機(jī)身縱向偏移方向。

在本發(fā)明的應(yīng)用場(chǎng)合中，掘進(jìn)機(jī)用于巷道掘進(jìn)，受掘進(jìn)機(jī)機(jī)身寬度、長(zhǎng)度及巷道寬度限制，掘進(jìn)機(jī)在巷道中的最大偏轉(zhuǎn)角度一般不超過10 度，即機(jī)身特征點(diǎn)A、B 連線與機(jī)身后方巷道頂板上設(shè)置的工業(yè)智能攝像機(jī)成像平面夾角一般不超過10 度，而機(jī)身與視覺定位定向儀之間的距離通常在數(shù)十米到上百米，因此，A、B 的像、（如圖3 所示）之間的距離因掘進(jìn)機(jī)偏轉(zhuǎn)（A、B 連線與工業(yè)智能攝像機(jī)成像平面夾角）產(chǎn)生的改變可忽略不計(jì)，因此可利用光學(xué)成像原理和三角法測(cè)距原理，在已知工業(yè)智能攝像機(jī)鏡頭焦距f、機(jī)身特征點(diǎn)間距離AB、CD、MN，且通過圖像處理算法獲得各特征點(diǎn)對(duì)應(yīng)像點(diǎn)、的位置坐標(biāo)時(shí)，機(jī)身與工業(yè)智能攝像機(jī)之間距離根據(jù)

可以估算出，從而各特征點(diǎn)A、B、C、D 在工業(yè)智能攝像機(jī)坐標(biāo)系中的橫向縱向二維空間坐標(biāo)根據(jù)

可算出，AB 和CD 的中點(diǎn)M、N 的橫向縱向二維空間坐標(biāo)根據(jù)

也可算出，因此，將MN 投影在水平面上，則MN，即掘進(jìn)機(jī)機(jī)身中軸線在工業(yè)智能攝像機(jī)坐標(biāo)系中的偏轉(zhuǎn)角根據(jù)

可計(jì)算出；同理，將MN 投影在鉛垂面上，則機(jī)身俯仰角根據(jù)

可計(jì)算出；A、B 的像點(diǎn)在工業(yè)智能攝像機(jī)坐標(biāo)系中與其橫軸的夾角即為掘進(jìn)機(jī)機(jī)身的橫滾角；

以MN 的中點(diǎn)為機(jī)身中心，則機(jī)身的縱向偏移、橫向偏移、與工業(yè)智能攝像機(jī)之間的距離即掘進(jìn)機(jī)機(jī)身的空間位置坐標(biāo)(x, y, z)根據(jù)

可計(jì)算出；

其中：A、B、C、D 為選定的特征點(diǎn)，、、、分別為A、B、C、D的像，AB、CD為機(jī)身特征點(diǎn)間距，MN為AB、CD的中點(diǎn)連線，f為工業(yè)智能攝像機(jī)鏡頭焦距。

本發(fā)明提出的一種基于機(jī)器視覺技術(shù)的四點(diǎn)式掘進(jìn)機(jī)自動(dòng)定位定向方法，以掘進(jìn)機(jī)機(jī)身四個(gè)特定位置作為特征點(diǎn)，由工業(yè)智能攝像機(jī)獲取其圖像，從中解算掘進(jìn)機(jī)機(jī)身相對(duì)于工業(yè)智能攝像機(jī)的偏轉(zhuǎn)角、橫滾角、俯仰角以及機(jī)身的橫向偏移量、縱向偏移量和機(jī)身到工業(yè)智能攝像機(jī)的距離等參數(shù)，實(shí)現(xiàn)掘進(jìn)機(jī)自動(dòng)定位定向，具有自動(dòng)化程度高，結(jié)構(gòu)簡(jiǎn)單，姿態(tài)信息完整、精度高等優(yōu)點(diǎn)。