[發(fā)明專利]一種懸臂式掘進(jìn)機(jī)空間位姿自動(dòng)檢測(cè)方法及系統(tǒng)

有效

• 申請(qǐng)?zhí)枺?/td>: 202010850770.0
• 申請(qǐng)日: 2020-08-21
• 公開（公告）號(hào): CN112050732B
• 公開（公告）日: 2022-03-15
• 發(fā)明（設(shè)計(jì)）人: 張旭輝;謝楠;張超;楊文娟;張楷鑫;周創(chuàng)
• 申請(qǐng)（專利權(quán)）人: 西安科技大學(xué)
• 主分類號(hào): G01B11/00
• 分類號(hào): G01B11/00
• 代理公司: 西安恒泰知識(shí)產(chǎn)權(quán)代理事務(wù)所 61216
• 代理人: 祁凡雨

1.一種懸臂式掘進(jìn)機(jī)空間位姿自動(dòng)檢測(cè)方法，用于實(shí)現(xiàn)懸臂式掘進(jìn)機(jī)截割臂相對(duì)巷道的實(shí)時(shí)位姿檢測(cè)，其特征在于，包括以下步驟：

步驟一，計(jì)算懸臂式掘進(jìn)機(jī)截割臂相對(duì)于懸臂式掘進(jìn)機(jī)機(jī)體的位姿；

步驟1.1，采集懸臂式掘進(jìn)機(jī)截割臂的標(biāo)靶圖像；

步驟1.2，提取步驟1.1得到標(biāo)靶圖像的光斑，用區(qū)域劃分法將所有光斑排序并采用最小二乘法進(jìn)行直線擬合，得到四條兩兩相交的直線，將每?jī)蓷l直線的交點(diǎn)作為特征點(diǎn)；

步驟1.3，根據(jù)特征點(diǎn)在標(biāo)靶坐標(biāo)系中的坐標(biāo)，采用對(duì)偶四元數(shù)誤差模型計(jì)算截割臂相對(duì)于機(jī)體的水平擺角θ1和垂直擺角θ2；

其中，所述標(biāo)靶坐標(biāo)系是指以標(biāo)靶圖像的中心為原點(diǎn)，X軸平行于標(biāo)靶上下邊指向右，Y平行于標(biāo)靶左右邊指向下，Z軸垂直于標(biāo)靶圖像平面；

步驟1.4，根據(jù)θ1和θ2確定截割臂相對(duì)于懸臂式掘進(jìn)機(jī)機(jī)體的位姿

其中，d為懸臂式掘進(jìn)機(jī)油缸的伸縮距離，b1為懸臂式掘進(jìn)機(jī)抬升關(guān)節(jié)與伸縮關(guān)節(jié)之間的高度差；a1為懸臂式掘進(jìn)機(jī)回轉(zhuǎn)臺(tái)中心與抬升關(guān)節(jié)之間水平距離，a2為懸臂式掘進(jìn)機(jī)抬升關(guān)節(jié)與伸縮關(guān)節(jié)之間的距離，a3為懸臂式掘進(jìn)機(jī)伸縮關(guān)節(jié)與截割臂之間的水平距離；

步驟二，確定截割臂相對(duì)巷道的實(shí)時(shí)位姿

其中，為采用視覺測(cè)量方法確定的懸臂式掘進(jìn)機(jī)機(jī)體相對(duì)于巷道的位姿；

步驟1.2，提取步驟1.1得到標(biāo)靶圖像的光斑，利用坐標(biāo)系中第一、二、三、四象限，將所獲取截割臂標(biāo)靶圖像的光斑分為四部分；對(duì)于各部分點(diǎn)按照光斑圖像的灰度值分別進(jìn)行排序，然后將所得點(diǎn)按照頂部、底部、左側(cè)、右側(cè)進(jìn)行集合，進(jìn)一步進(jìn)行直線擬合，得到四條兩兩相交的直線，將每?jī)蓷l直線的交點(diǎn)作為特征點(diǎn)；利用四點(diǎn)透視成像方法得到所有特征點(diǎn)在相機(jī)坐標(biāo)系中的坐標(biāo)；

步驟1.3，根據(jù)特征點(diǎn)在標(biāo)靶坐標(biāo)系中的坐標(biāo)以及步驟1.2得到特征點(diǎn)在相機(jī)坐標(biāo)系中坐標(biāo)，采用對(duì)偶四元數(shù)誤差模型計(jì)算截割臂相對(duì)于機(jī)體的水平擺角θ1和垂直擺角θ2；

所述相機(jī)坐標(biāo)系是指以相機(jī)的光心為原點(diǎn)，X軸沿水平方向，Y軸沿豎直方向，Z軸垂直Y、X軸指向標(biāo)靶方向，所述相機(jī)設(shè)置在懸臂式掘進(jìn)機(jī)機(jī)體上部?jī)蓚?cè)；

其中，機(jī)體相對(duì)于巷道的位姿的計(jì)算如下：

式中，為相機(jī)相對(duì)于激光指向儀的位姿；為激光指向儀在巷道中的位姿，通過(guò)全站儀測(cè)量得到；為相機(jī)相對(duì)于機(jī)體的外參矩陣，通過(guò)相機(jī)外參標(biāo)定得到；

其中，的計(jì)算步驟如下：

分別提取相機(jī)采集的兩個(gè)激光束圖像，對(duì)激光束圖像中的激光光斑信息分別進(jìn)行特征提取得到兩個(gè)光斑區(qū)域并采用最小二乘法進(jìn)行直線擬合，分別獲得兩個(gè)直線方程；同時(shí)通過(guò)兩個(gè)光斑區(qū)域分別獲得兩個(gè)光斑中心點(diǎn)；通過(guò)兩個(gè)光斑中心點(diǎn)的連線結(jié)合兩個(gè)直線方程建立懸臂式掘進(jìn)機(jī)位姿解算模型，確定相機(jī)相對(duì)于激光指向儀的位姿

2.一種懸臂式掘進(jìn)機(jī)空間位姿自動(dòng)檢測(cè)系統(tǒng)，該系統(tǒng)用于實(shí)現(xiàn)懸臂式掘進(jìn)機(jī)截割臂相對(duì)巷道的實(shí)時(shí)位姿檢測(cè)，其特征在于，包括：

通信模塊，包括相機(jī)，用于采集懸臂式掘進(jìn)機(jī)截割臂上的標(biāo)靶圖像以及懸臂式掘進(jìn)機(jī)運(yùn)行狀態(tài)信息，所述相機(jī)設(shè)置在懸臂式掘進(jìn)機(jī)機(jī)體上部?jī)蓚?cè)；所述懸臂式掘進(jìn)機(jī)運(yùn)行狀態(tài)信息包括懸臂式掘進(jìn)機(jī)油缸的伸縮距離，懸臂式掘進(jìn)機(jī)抬升關(guān)節(jié)與伸縮關(guān)節(jié)之間的高度差，懸臂式掘進(jìn)機(jī)回轉(zhuǎn)臺(tái)中心與抬升關(guān)節(jié)之間水平距離，懸臂式掘進(jìn)機(jī)抬升關(guān)節(jié)與伸縮關(guān)節(jié)之間的距離，懸臂式掘進(jìn)機(jī)伸縮關(guān)節(jié)與截割臂之間的水平距離以及懸臂式機(jī)體距離前煤壁的距離；

視覺測(cè)量模塊，用于測(cè)量懸臂式掘進(jìn)機(jī)機(jī)體相對(duì)于巷道的位姿，以多點(diǎn)紅外LED和兩條平行激光束為特征進(jìn)行視覺測(cè)量，包括紅外LED標(biāo)靶和兩個(gè)激光指向儀，紅外標(biāo)靶設(shè)置在截割臂上部，用于提取相機(jī)采集的標(biāo)靶圖像的光斑；兩個(gè)激光指向儀設(shè)置在巷道內(nèi)上部，用于沿著巷道頂板發(fā)出兩條相互平行的紅色激光束；所述相機(jī)還用于采集激光指向儀的激光束圖像；

處理器，存儲(chǔ)有計(jì)算機(jī)程序，用于按照權(quán)利要求1中步驟一的內(nèi)容計(jì)算懸臂式掘進(jìn)機(jī)截割臂相對(duì)于懸臂式掘進(jìn)機(jī)機(jī)體的位姿以及按照權(quán)利要求1中步驟二的內(nèi)容計(jì)算截割臂相對(duì)于巷道的實(shí)時(shí)位姿；

該系統(tǒng)還包括位姿顯示模塊，用于將懸臂式掘進(jìn)機(jī)截割臂以及機(jī)體相對(duì)巷道的位姿進(jìn)行實(shí)時(shí)圖形化界面顯示；

該系統(tǒng)還用于實(shí)現(xiàn)懸臂式掘進(jìn)機(jī)機(jī)體相對(duì)于巷道的位姿檢測(cè)，機(jī)體相對(duì)于巷道的位姿的計(jì)算如下：

式中，為相機(jī)相對(duì)于激光指向儀的位姿；為激光指向儀在巷道中的位姿，通過(guò)全站儀測(cè)量得到；為相機(jī)相對(duì)于機(jī)體的外參矩陣，通過(guò)相機(jī)外參標(biāo)定得到；

其中，的計(jì)算步驟如下：

分別提取相機(jī)采集的兩個(gè)激光束圖像，對(duì)激光束圖像中的激光光斑信息分別進(jìn)行特征提取得到兩個(gè)光斑區(qū)域并采用最小二乘法進(jìn)行直線擬合，分別獲得兩個(gè)直線方程；同時(shí)通過(guò)兩個(gè)光斑區(qū)域分別獲得兩個(gè)光斑中心點(diǎn)；通過(guò)兩個(gè)光斑中心點(diǎn)的連線結(jié)合兩個(gè)直線方程建立懸臂式掘進(jìn)機(jī)位姿解算模型，確定相機(jī)相對(duì)于激光指向儀的位姿

確定相機(jī)相對(duì)于激光指向儀的位姿具體計(jì)算如下：

其中，α1、β1、γ1分別為相機(jī)坐標(biāo)系在激光指向儀坐標(biāo)系下繞x軸、y軸、z軸旋轉(zhuǎn)的角度，X1、Y1、Z1分別為相機(jī)坐標(biāo)系在激光指向儀坐標(biāo)系下x軸、y軸、z軸三個(gè)方向的分量；所述激光指向儀坐標(biāo)系的原點(diǎn)在兩個(gè)激光指向儀連線的中心位置；所述相機(jī)坐標(biāo)系的原點(diǎn)在相機(jī)質(zhì)心位置，激光指向儀坐標(biāo)系與相機(jī)坐標(biāo)系的各個(gè)坐標(biāo)軸同向。