基于旋轉(zhuǎn)扇面激光的懸臂式掘進(jìn)機(jī)位姿測(cè)量方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明屬于工程機(jī)械設(shè)備位姿測(cè)量技術(shù)領(lǐng)域，特別涉及一種對(duì)煤礦用懸臂式掘進(jìn) 機(jī)的位置和姿態(tài)進(jìn)行測(cè)量，進(jìn)而對(duì)懸臂式掘進(jìn)機(jī)掘進(jìn)方向進(jìn)行精確指向。
【背景技術(shù)】
[0002] 傳統(tǒng)的懸臂式掘進(jìn)機(jī)位姿測(cè)量方法是人工目視法，懸臂式掘進(jìn)機(jī)在煤巷中唯一的 參考來(lái)自于點(diǎn)激光指向儀。在懸臂式掘進(jìn)機(jī)后方安放1臺(tái)點(diǎn)激光指向儀（指示巷道設(shè)計(jì)方 向），其在煤巷前部掘進(jìn)斷面形成一個(gè)光斑，懸臂式掘進(jìn)機(jī)司機(jī)在作業(yè)中須以肉眼觀察掘進(jìn) 斷面上光斑的位置，時(shí)時(shí)注意巷道走向是否正確。該方法精度低，操作復(fù)雜，耗費(fèi)時(shí)間，可靠 性差，全憑操作司機(jī)的經(jīng)驗(yàn)來(lái)保證巷道成型質(zhì)量。
[0003] 隨著技術(shù)的發(fā)展，出現(xiàn)了大量的現(xiàn)代化位姿測(cè)量方式，如自動(dòng)全站儀位姿測(cè)量系 統(tǒng)和慣性導(dǎo)航位姿測(cè)量系統(tǒng)。對(duì)于自動(dòng)全站儀位姿測(cè)量系統(tǒng)，機(jī)器人在地下行進(jìn)過(guò)程中可 以自動(dòng)跟蹤標(biāo)靶，對(duì)目標(biāo)點(diǎn)進(jìn)行實(shí)時(shí)連續(xù)檢測(cè)。但全站儀使用激光束作為測(cè)量介質(zhì)來(lái)對(duì)空 間中某點(diǎn)進(jìn)行定位，只能實(shí)現(xiàn)點(diǎn)對(duì)點(diǎn)的測(cè)量，抗遮擋能力差，煤礦掘進(jìn)工作面設(shè)備雜多，且 煤巷走向起伏不平，通視情況差，故使用全站儀的話其激光束很容易被遮擋。對(duì)于慣性導(dǎo)航 系統(tǒng)，定位誤差會(huì)隨時(shí)間而增大，不適用于長(zhǎng)距離長(zhǎng)時(shí)間的測(cè)量，需要定期的矯正。而煤礦 掘進(jìn)工作面一般位于地下幾十至幾百米處，無(wú)法使用GPS等定位技術(shù)來(lái)對(duì)慣性導(dǎo)航系統(tǒng)進(jìn) 行校正。故絕大部分現(xiàn)代化的位姿測(cè)量方式不適用于懸臂式掘進(jìn)機(jī)位姿測(cè)量。

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

[0004] 本發(fā)明的目的在于提出一種利用基于扇面激光測(cè)角的空間點(diǎn)三維坐標(biāo)測(cè)量系統(tǒng) 進(jìn)行懸臂式掘進(jìn)機(jī)位姿測(cè)量的方法，可實(shí)現(xiàn)對(duì)懸臂式掘進(jìn)機(jī)的位置和姿態(tài)進(jìn)行測(cè)量，大大 提高了對(duì)懸臂式掘進(jìn)機(jī)位姿的測(cè)量精確性，為懸臂式掘進(jìn)機(jī)控制提供精確數(shù)據(jù)，以提高巷 道成型質(zhì)量。
[0005] 本發(fā)明的技術(shù)方案：利用基于扇面激光測(cè)角的空間點(diǎn)三維坐標(biāo)測(cè)量系統(tǒng)進(jìn)行懸臂 式掘進(jìn)機(jī)位姿測(cè)量，包含以下步驟：
[0006] (1)將扇面激光發(fā)射端安裝到懸臂式掘進(jìn)機(jī)上，使其與懸臂式掘進(jìn)機(jī)固連。扇面激 光接收端安裝到已成型的巷道中，即3個(gè)不共線的扇面激光接收器。
[0007] (2)由扇面激光發(fā)射端構(gòu)建一坐標(biāo)系，稱為機(jī)身坐標(biāo)系（O-XYZ)。扇面激光發(fā)射端 由高精度滑臺(tái)、高精度轉(zhuǎn)臺(tái)、扇面激光發(fā)射器、起始基準(zhǔn)組成，設(shè)扇面激光發(fā)射器初始位置 旋轉(zhuǎn)軸為Z軸，高精度滑臺(tái)移動(dòng)方向?yàn)閄軸。
[0008] (3)由激光接收端，即3個(gè)不共線的扇面激光接收器構(gòu)建一坐標(biāo)系，稱為巷道坐標(biāo) 系（O' -X' Y' Z，）。
[0009] (4)在懸臂式掘進(jìn)機(jī)處于截割狀態(tài)時(shí)，扇面激光發(fā)射器在高精度滑臺(tái)和轉(zhuǎn)臺(tái)的 作用下，在兩個(gè)位置（初始位置和移動(dòng)位置）發(fā)射出三束扇面激光交匯到一點(diǎn)。依次測(cè) 得3個(gè)扇面激光接收器在機(jī)身坐標(biāo)系（O-XYZ)的三維坐標(biāo)值，即（ΧρΥρΖ)、（x 2, y2, Z2)、 (X3, y3，Z3)。
[0010] (5)如圖I所示，由3個(gè)扇面激光接收器的三維坐標(biāo)值計(jì)算得巷道坐標(biāo)系（O' -Xr Y1 )相對(duì)于機(jī)身坐標(biāo)系（O-XYZ)的各變換參數(shù)。T1為巷道坐標(biāo)系（(V -Xr Y1 Z')的r軸在機(jī)身坐標(biāo)系（O-XYZ)下的方向向量，T2、T^別為r軸的方向向量， X。為巷道坐標(biāo)系（CV -X' Y' Z')的原點(diǎn)在機(jī)身坐標(biāo)系（O-XYZ)下的坐標(biāo)。
[0011] 從而獲得平移矩陣D1和旋轉(zhuǎn)矩陣R lt)
[0012] (6)用礦用激光全站儀測(cè)得3個(gè)扇面激光接收器相對(duì)于空間大地坐標(biāo)系的三維坐 標(biāo)值，同樣按照上述方法求得巷道坐標(biāo)系（CV -X' Y' Z')與空間大地坐標(biāo)系的平移矩 陣D2和旋轉(zhuǎn)矩陣R 2，以及巷道坐標(biāo)系原點(diǎn)在大地坐標(biāo)系下的坐標(biāo)（a，b，C)。
[0013] (7)將扇面激光發(fā)射端上固定三點(diǎn)，即機(jī)身坐標(biāo)系（O-XYZ)上不共線固定3點(diǎn)變換 至空間大地坐標(biāo)系中，假設(shè)所取三點(diǎn)坐標(biāo)分別為（100,0, 0)、（0, 100, 0)、（〇,〇, 100)，單位為 mm。以（100,0,0)為例，變換公式如下：
[0014] (X，Y，Z，1) = (100,0,0, 1) · D1 · R1 · D2 · R2
[0015] (X，Υ，Ζ)即為機(jī)身坐標(biāo)系中（100,0,0)在空間大地坐標(biāo)系中的坐標(biāo)值，依照上述 公式分別求出（〇, 100, 〇)、（〇,〇, 100)進(jìn)行坐標(biāo)系變換后的坐標(biāo)值。
[0016] 由于扇面激光發(fā)射端與懸臂式掘進(jìn)機(jī)固聯(lián)，故扇面激光發(fā)射端上固定三點(diǎn)就是懸 臂式掘進(jìn)機(jī)上固定三點(diǎn)。
[0017] (8)根據(jù)懸臂式掘進(jìn)機(jī)上不共線固定三點(diǎn)相對(duì)于空間大地坐標(biāo)系的三維坐標(biāo)值， 即可表示懸臂式掘進(jìn)機(jī)在空間大地坐標(biāo)系內(nèi)的位置和姿態(tài)情況，由此實(shí)現(xiàn)對(duì)懸臂式掘進(jìn)機(jī) 的定位和姿態(tài)測(cè)量，為其進(jìn)一步掘進(jìn)提供精確數(shù)據(jù)。
[0018] 本發(fā)明的有益效果：①基于旋轉(zhuǎn)扇面激光測(cè)角的空間點(diǎn)三維坐標(biāo)測(cè)量系統(tǒng)是一種 全自動(dòng)化、高可靠性、高精度、低成本的三維坐標(biāo)測(cè)量系統(tǒng)，懸臂式掘進(jìn)機(jī)在煤巷掘進(jìn)時(shí)，將 本系統(tǒng)的扇面激光接收端（3個(gè)扇面激光接收器）固定在已掘巷道固定點(diǎn)處，將扇面激光發(fā) 射端安裝在懸臂式掘進(jìn)機(jī)上，使其與懸臂式掘進(jìn)機(jī)固聯(lián)，實(shí)現(xiàn)對(duì)懸臂式掘進(jìn)機(jī)位姿狀態(tài)的 高精度、高自動(dòng)化測(cè)量；②可達(dá)到Icm的定位精度和優(yōu)于0. Γ的姿態(tài)檢測(cè)精度，完全滿足 對(duì)懸臂式掘進(jìn)機(jī)的位姿測(cè)量需求，極大地提高了煤巷成型質(zhì)量。
【附圖說(shuō)明】
[0019] 圖1為本發(fā)明實(shí)施例中懸臂式掘進(jìn)機(jī)位姿測(cè)量過(guò)程示意圖。
[0020] 1 :扇面激光發(fā)射器 2 :高精度轉(zhuǎn)臺(tái)；
[0021] 3:高精度滑臺(tái) 4:機(jī)身坐標(biāo)系
[0022] 5 :巷道坐標(biāo)系 6 :扇面激光接收器1
[0023] 7 :扇面激光接收器2 8 :扇面激光接收器3
[0024] 9 :空間大地坐標(biāo)系
【具體實(shí)施方式】
[0025] 如圖1所示，利用基于扇面激光測(cè)角的空間點(diǎn)三維坐標(biāo)測(cè)量系統(tǒng)進(jìn)行懸臂式掘進(jìn) 機(jī)位姿測(cè)量的方法，位姿測(cè)量過(guò)程包含以下步驟：
[0026] (1)將扇面激光發(fā)射端安裝到懸臂式掘進(jìn)機(jī)上，使其與懸臂式掘進(jìn)機(jī)固連。扇面激 光接收端安裝到已成型的巷道中，即3個(gè)不共線的扇面激光接收器。
[0027] (2)由扇面激光發(fā)射端構(gòu)建一坐標(biāo)系，稱為機(jī)身坐標(biāo)系（O-XYZ)。扇面激光發(fā)射端 由高精度滑臺(tái)、高精度轉(zhuǎn)臺(tái)、扇面激光發(fā)射器、起始基準(zhǔn)組成，設(shè)扇面激光發(fā)射器初始位置 旋轉(zhuǎn)軸為Z軸，高精度滑臺(tái)移動(dòng)方向?yàn)閄軸。
[0028] (3)由激光接收端，即3個(gè)不共線的扇面激光接收器構(gòu)建一坐標(biāo)系，稱為巷道坐標(biāo) 系（O' -X' Y' Z，）。
[0029] (4)在懸臂式掘進(jìn)機(jī)處于截割狀態(tài)時(shí)，扇面激光發(fā)射器在高精度滑臺(tái)和轉(zhuǎn)臺(tái)的作 用下，在兩個(gè)位置（初始位置和移動(dòng)位置）發(fā)射出三束扇面激光交匯到一點(diǎn)。依次測(cè)得3 個(gè)扇面激光接收器在機(jī)身坐標(biāo)系（O-XYZ)的三維坐標(biāo)值，即扇面激光接收器I :(Xl，yi，Z1)， 扇面激光接收器2 : (x2, y2, z2)，扇面激光接收器3 : (x3, y3, z3)。
[0030] (5)如圖1所示，由3個(gè)扇面激光接收器的三維坐標(biāo)值計(jì)算得巷道坐標(biāo)系（0' -Xr Y' )相對(duì)于機(jī)身坐標(biāo)系（O-XYZ)的各變換參數(shù)。T1為巷道坐標(biāo)系（(V -Xr Y1 Z')的t軸在機(jī)身坐標(biāo)系（O-XYZ)下的方向向量，T2、T^別為t軸的方向向量， X。為巷道坐標(biāo)系（CV -X' Y' Z')的原點(diǎn)在機(jī)身坐標(biāo)系（O-XYZ)下的坐標(biāo)。
[0031] 因表達(dá)式過(guò)于復(fù)雜，故用t來(lái)代替部分表達(dá)式，設(shè)：
【主權(quán)項(xiàng)】
1. 一種用于煤礦掘進(jìn)工作面的懸臂式掘進(jìn)機(jī)位姿測(cè)量方法，其特征在于位姿測(cè)量過(guò)程 包含以下步驟： (1) 將扇面激光發(fā)射端安裝到懸臂式掘進(jìn)機(jī)上，使其與懸臂式掘進(jìn)機(jī)固連。扇面激光接 收端安裝到已成型的巷道中，即3個(gè)不共線的扇面激光接收器。 (2) 由扇面激光發(fā)射端構(gòu)建一坐標(biāo)系，稱為機(jī)身坐標(biāo)系（O-XYZ)。扇面激光發(fā)射端由高 精度滑臺(tái)、高精度轉(zhuǎn)臺(tái)、扇面激光發(fā)射器、起始基準(zhǔn)組成，設(shè)扇面激光發(fā)射器初始位置旋轉(zhuǎn) 軸為Z軸，高精度滑臺(tái)移動(dòng)方向?yàn)閄軸。 (3) 由激光接收端，即3個(gè)不共線的扇面激光接收器構(gòu)建一坐標(biāo)系，稱為巷道坐標(biāo)系 (0，-X，Y，Z，）。 (4)在懸臂式掘進(jìn)機(jī)處于截割狀態(tài)時(shí)，扇面激光發(fā)射器在高精度滑臺(tái)和轉(zhuǎn)臺(tái)的作用下， 在兩個(gè)位置（初始位置和移動(dòng)位置）發(fā)射出三束扇面激光交匯到一點(diǎn)。依次測(cè)得3個(gè)扇面 激光接收器在機(jī)身坐標(biāo)系（O-XYZ)的三維坐標(biāo)值，即（X1,Y1,Z1)、（x2,y2,z2)、（x3,y3,Z3)。 (5) 如圖1所示，由3個(gè)扇面激光接收器的三維坐標(biāo)值計(jì)算得巷道坐標(biāo)系 ((T-X'Y'Z')相對(duì)于機(jī)身坐標(biāo)系（O-XYZ)的各變換參數(shù)。T1為巷道坐標(biāo)系 (O' -X^ )的t軸在機(jī)身坐標(biāo)系（O-XYZ)下的方向向量，T2、T3*別為t、Z' 軸的方向向量，Xtl為巷道坐標(biāo)系（〇' -X'Y'Z')的原點(diǎn)在機(jī)身坐標(biāo)系（O-XYZ)下的坐 標(biāo)。 從而獲得平移矩陣D1和旋轉(zhuǎn)矩陣Rlt) (6) 用礦用激光全站儀測(cè)得3個(gè)扇面激光接收器相對(duì)于空間大地坐標(biāo)系的三維坐標(biāo) 值，同樣按照上述方法求得巷道坐標(biāo)系（〇' -X'Y'Z')與空間大地坐標(biāo)系的平移矩陣 D2和旋轉(zhuǎn)矩陣R2，以及巷道坐標(biāo)系原點(diǎn)在大地坐標(biāo)系下的坐標(biāo)（a，b，c)。 (7) 將扇面激光發(fā)射端上固定三點(diǎn)，即機(jī)身坐標(biāo)系（O-XYZ)上不共線固定3點(diǎn)變換至空 間大地坐標(biāo)系中，假設(shè)所取三點(diǎn)坐標(biāo)分別為（100,0, 0)、（0, 100, 0)、（0,0,100)，單位為mm。 以（100, 0,0)為例，變換公式如下： (X，Y，Z，1) = (100,0,0, 1) ?D1 ?R1 ?D2 ?R2 (X，Y，Z)即為機(jī)身坐標(biāo)系中（100,0,0)在空間大地坐標(biāo)系中的坐標(biāo)值，依照上述公式 分別求出（〇, 100, 〇)、（〇,〇, 100)進(jìn)行坐標(biāo)系變換后的坐標(biāo)值。 由于扇面激光發(fā)射端與懸臂式掘進(jìn)機(jī)固聯(lián)，故扇面激光發(fā)射端上固定三點(diǎn)就是懸臂式 掘進(jìn)機(jī)上固定三點(diǎn)。 (8) 根據(jù)懸臂式掘進(jìn)機(jī)上不共線固定三點(diǎn)相對(duì)于空間大地坐標(biāo)系的三維坐標(biāo)值，即可 表示懸臂式掘進(jìn)機(jī)在空間大地坐標(biāo)系內(nèi)的位置和姿態(tài)情況，由此實(shí)現(xiàn)對(duì)懸臂式掘進(jìn)機(jī)的定 位和姿態(tài)測(cè)量，為其進(jìn)一步掘進(jìn)提供精確數(shù)據(jù)。
【專利摘要】本發(fā)明涉及一種用于煤礦掘進(jìn)工作面的懸臂式掘進(jìn)機(jī)位姿測(cè)量方法，充分利用基于旋轉(zhuǎn)扇面激光測(cè)角的空間點(diǎn)三維坐標(biāo)測(cè)量系統(tǒng)，即將該系統(tǒng)的扇面激光發(fā)射端安裝在懸臂式掘進(jìn)機(jī)機(jī)身上，扇面激光接收端（包含3個(gè)不共線的扇面激光接收器）固定安裝在已經(jīng)由懸臂式掘進(jìn)機(jī)掘出的煤巷后方，扇面激光接收器相對(duì)于空間大地坐標(biāo)系的坐標(biāo)值由其它測(cè)量方式測(cè)得。由扇面激光發(fā)射端測(cè)得扇面激光接收器相對(duì)于發(fā)射端的坐標(biāo)值，再通過(guò)相應(yīng)的坐標(biāo)轉(zhuǎn)換，計(jì)算出懸臂式掘進(jìn)機(jī)上固定三點(diǎn)（不共線）相對(duì)于空間大地坐標(biāo)系的三維坐標(biāo)值。若已知空間剛體上固定三點(diǎn)（不共線）相對(duì)于空間大地坐標(biāo)系的三維坐標(biāo)值，可計(jì)算出該空間剛體的位姿（位置和姿態(tài)）數(shù)據(jù)。
【IPC分類】G01C15-00, G01C21-00
【公開(kāi)號(hào)】CN104729501
【申請(qǐng)?zhí)枴緾N201510121265
【發(fā)明人】吳淼, 賈文浩, 符世琛, 陶云飛, 張敏駿, 宗凱, 薛光輝, 楊健健
【申請(qǐng)人】中國(guó)礦業(yè)大學(xué)(北京)
【公開(kāi)日】2015年6月24日
【申請(qǐng)日】2015年3月19日