光學靶標遮擋條件下掘進機定位解算方法

摘要：針對目前常用的基于慣導+視覺測量+光學靶標的掘進機組合式導航定位存在的光學靶標被遮擋情況下掘進機定位中斷問題，提出了一種光學靶標遮擋條件下掘進機定位解算方法。首先，采集4 個呈矩形分布的靶標點組成的光學靶標在無遮擋情況下的圖像，得到靶標點在相機內成像光斑的像素坐標并構造成矩形，再按照一定比例擴大構造輔助矩形區(qū)域框。其次，采集部分靶標點被遮擋情況下的圖像，得到無遮擋靶標點在相機內成像光斑的像素坐標，根據靶標點的成像光斑與輔助矩形區(qū)域框頂點的歐氏距離，確定無遮擋靶標點與成像光斑的對應關系，進而確定被遮擋的靶標點。然后，利用已知的靶標幾何尺寸和慣導提供的靶標姿態(tài)信息，建立投影后的靶標點與成像光斑的對應關系，進而求解出被遮擋靶標點對應的光斑像素坐標。最后，利用N 點位姿透視求解（PNP）算法求得光學靶標中心位置的空間坐標，實現掘進機定位解算。試驗結果表明，光學靶標被遮擋情況下，通過推算被遮擋靶標點對應的光斑像素坐標，可以解決掘進機定位中斷問題，保證了掘進機定位的實時性，且定位誤差滿足掘進機實際定位需求。

關鍵詞：掘進機定位；光學靶標；慣導；視覺測量；組合導航

中圖分類號：TD632 文獻標志碼：A

0 引言

掘進機導航定位系統(tǒng)是掘進機智能化的關鍵組成部分[1-3]。目前掘進機導航定位技術主要有單一測量導航技術和組合導航技術2 類。在單一測量導航技術研究方面：張國喜[4]采用慣導設備直接測量掘進機位姿；田原[5]采用基于相機+光學靶標的視覺測量方法對掘進機機身進行定位測量；劉超等[6]基于超寬帶測距和TSOA（Time Sum of Arrival，波達時間和）定位原理對掘進機機身姿態(tài)進行測量；石勇[7]提出了基于3D 激光雷達的動態(tài)測距、測向方法，實時檢測掘進機相對巷道中線的位置偏移和角度偏移；薛光輝等[8]提出了基于激光靶向跟蹤的掘進機位姿測量方法；楊文娟等[9-10]提出了一種基于激光束特征的懸臂式掘進機機身測量系統(tǒng)。單一測量導航技術僅能提供單一的位置或姿態(tài)測量功能，在能見度低、空間狹窄的環(huán)境下適用性較差。組合導航技術以慣導技術為核心，同時輔助其他測量手段。在組合導航技術研究方面：劉豪[11]將捷聯慣導與里程計組合，構建了掘進機自主導航定位系統(tǒng)，但掘進機履帶打滑造成里程計空轉，進而引入較大的定位誤差；張旭輝等[12]采用全站儀與捷聯慣導進行聯合定位，通過卡爾曼濾波融合全站儀與捷聯慣導測量數據，實現掘進機精確定位，但井下大范圍移站時全站儀操作過于復雜，難以廣泛推廣；崔玉明[13]提出了掘進機視覺/慣性融合定位策略，通過視覺測量系統(tǒng)自動識別并測量環(huán)境特征量，但井下光照不足、粉塵濃度大等因素對視覺測量造成影響；為進一步提升視覺測量的抗干擾能力，Yang Wenjuan等[14]、雷孟宇等[15]、黃東等[16]提出了直接在掘進機機身上設置光學靶標，利用視覺測量系統(tǒng)在掘進機后端對靶標進行測量。光學靶標自身可發(fā)光、光束可透粉塵等特性有效克服了光照不足、粉塵濃度大等環(huán)境因素的影響，因此慣導+視覺測量+光學靶標組合的掘進機導航定位成為目前廣泛應用的技術方案[17]。

通常情況下，光學靶標由固定在同一平面上、排列參數已知的一系列靶標點組成，最常見的形式是由4 個呈矩形分布的靶標點組成。光學靶標與慣導組成的掘進機位姿測量裝置固定在掘進機尾部，并且朝向視覺測量設備。視覺測量設備采集到光學靶標的圖像后，獲得圖像中靶標點對應的光斑像素坐標， 采用N 點位姿透視求解（Perspective-N-Point，PNP）算法計算得到光學靶標中心位置坐標，并將該坐標信息發(fā)送至慣導系統(tǒng)進行組合導航解算，得到掘進機的位置和姿態(tài)信息。但由于掘進工作面經常會出現其他設備（如電纜、錨桿鉆臂、風筒等）遮擋靶標點的情況，此時相機采集的光斑圖像與靶標點未被遮擋時采集的光斑圖像不符，進而造成PNP 算法無法正常解算，導致掘進機定位中斷。

本文以光學靶標、視覺測量系統(tǒng)、慣導系統(tǒng)構成的掘進機組合式導航定位系統(tǒng)為研究對象，提出了一種光學靶標遮擋條件下掘進機定位解算方法。掘進機組合式導航定位系統(tǒng)獲取無遮擋情況下的光學靶標圖像并將圖像中的光斑進行標記，當下一幀圖像中部分靶標點被遮擋后，通過幀間圖像匹配，得到被遮擋靶標點的圖像信息，利用慣導提供的靶標姿態(tài)信息，建立投影后的靶標點與光斑的對應關系，進而求解出被遮擋靶標點對應的光斑像素坐標，從而解決靶標被遮擋情況下掘進機定位中斷的問題。

1 掘進機組合式導航定位原理

掘進機組合式導航定位系統(tǒng)采用慣導+視覺測量+光學靶標的定位方案，由懸掛在掘進機后端巷道頂板上的激光導引裝置和安裝在掘進機上的位姿測量裝置組成，如圖1 所示。激光導引裝置包含測量相機、指向激光器、姿態(tài)測量模塊和調整機構。通過預先標定，指向激光器的激光束與相機坐標系OcXcYcZc 的Yc 軸重合。位姿測量裝置包含光學靶標、慣導系統(tǒng)和數據解算模塊。光學靶標由4 個呈矩形分布的靶標點組成，光學靶標坐標系OtXtYtZt 與慣導坐標系OiXiYiZi 相平行。

激光導引裝置的相機坐標系OcXcYcZc 的Yc 軸與巷道掘進方向一致， 且Yc 軸位于巷道中軸面上，Xc 軸與巷道寬度方向一致，Zc 軸與巷道高度方向一致。位姿測量裝置中的光學靶標朝向激光導引裝置，且光學靶標坐標系OtXtYtZt、慣導坐標系OiXiYiZi與掘進機機身坐標系OrXrYrZr 完全平行。因此慣導系統(tǒng)提供的航向角、橫滾角和俯仰角代表光學靶標和掘進機機身的航向角、橫滾角和俯仰角，通過與地測部門提供的巷道航向角作差，可得掘進機機身相對于巷道坐標系的航向角、俯仰角和橫滾角。

激光導引裝置的測量相機檢測光學靶標上光斑坐標，采用PNP 算法對光學靶標進行坐標定位，即確定光學靶標坐標系原點Ot（光學靶標中心點）在相機坐標系中的坐標。由于相機坐標系與巷道坐標系平行，所以該定位坐標也反映了光學靶標中心點在巷道中的坐標。激光導引裝置解算得到的初步定位結果通過無線方式傳輸至位姿測量裝置中的慣導系統(tǒng)，由于光學靶標坐標系、慣導坐標系與掘進機機身坐標系完全平行，所以慣導系統(tǒng)內部可直接采用初步定位結果進行組合導航解算，并通過坐標位置推算出掘進機機身坐標系原點Or 在巷道內的準確坐標及掘進機機身的航向角、俯仰角和橫滾角。

2 光學靶標遮擋條件下掘進機定位解算方法

掘進機組合式導航定位系統(tǒng)的光學靶標與慣導系統(tǒng)都集成在位姿測量裝置中，因此當光學靶標中部分靶標點被遮擋時，可根據已知的靶標幾何尺寸結合慣導系統(tǒng)提供的靶標姿態(tài)信息，對被遮擋靶標點對應的光斑像素坐標進行推算，進而采用PNP 算法求得光學靶標中心位置的空間坐標，實現掘進機定位解算。

2.1 光斑標記

相機采集無遮擋條件下光學靶標圖像，通過灰度閾值處理并識別4 個光斑的位置，按順時針方向將4 個光斑依次標記為A，B，C，D，如圖2 所示。使用高斯擬合方法[18-20]獲得A，B，C，D4 個光斑對應的像素坐標， 分別為（μA，vA），（μB，vB），（μC，vC），（μD，vD）。

將4 個光斑構造成矩形G，然后將G 按一定比例擴大，構造輔助矩形區(qū)域框。將輔助矩形區(qū)域框4 個頂點按順時針方向設為O1， O2， O3， O4，其對應像素坐標分別為（μO1，vO1） ，（μO2，vO2） ，（μO3，vO3） ，（μO4，vO4） 。

2.2 光斑匹配

由于掘進機移動速率較低，4 個靶標點在下一幀圖像中的光斑像素位置不會發(fā)生劇烈變化。在獲取下一幀圖像后，可以參考上一幀圖像中構造的輔助矩形區(qū)域框。相較于其他光斑，A 點對應的靶標點在下一幀圖像中的光斑應與輔助矩形區(qū)域框頂點O1 的歐氏距離最小，如圖3 所示。將下一幀圖像中檢測出來的4 個光斑依次標記為A′，B′，C′，D′。如果光學靶標的部分靶標點被遮擋，則可以根據輔助矩形區(qū)域框頂點的關系確定缺失的光斑標記。

2.3 無遮擋情況下靶標點成像投影關系

已知光學靶標上4 個靶標點TA，TB，TC，TD 構成長為2a、寬為2b 的矩形靶面，如圖4 所示。在光學靶標坐標系OtXtYtZt 中， 4 個靶標點的坐標分別為TA（?a， 0， b） ， TB（a， 0， b） ， TC（a， 0， ?b） ， TD（?a， 0，?b） 。光學靶標坐標系OtXtYtZt 與慣導坐標系OiXiYiZi 完全平行，利用慣導坐標系OiXiYiZi 與相機坐標系OcXcYcZc 的夾角，可將由TA，TB，TC，TD 構成的光學靶標坐標系OtXtYtZt 通過姿態(tài)矩陣變換生成與相機坐標系OcXcYcZc 平行的由TA′，TB′，TC′，TD′，構成的投影靶面坐標系Ot_PXt_PYt_PZt_P， 其中TA′，TB′，TC′，TD′分別為4 個靶標點TA，TB，TC，TD在投影靶面坐標系下的對應投影點。

光學靶標坐標系OtXtYtZt 下的靶標點TA，TB，TC，TD 在投影靶面坐標系Ot_PXt_PYt_PZt_P 中的投影點TA′，TB′，TC′，TD′可表示為

式中Rtb為實際靶面坐標系到投影靶面坐標系的轉換矩陣。

式中γ，α，β分別為慣導系統(tǒng)提供的航向角、俯仰角、橫滾角。

光學靶標在經過姿態(tài)變換后，投影到與相機成像平面（相機坐標系XcOcZc 平面）平行的投影靶面坐標系上的投影點坐標，用于后續(xù)光斑像素坐標的推算。

2.4 被遮擋靶標點對應的光斑像素坐標推算

下面對檢測到3 個光斑和2 個光斑的情況分別進行缺失光斑像素坐標推算。

2.4.1 1 個靶標點被遮擋時光斑像素坐標推算

如果光學靶標中的1 個靶標點被遮擋，則在下一幀圖像中的輔助矩形區(qū)域框內將找到3 個光斑。對檢測到的3 個光斑進行標記，標記結果為A， B，C 或A，B，D 或A，C，D 或B，C，D 4 種情況。

以光斑標記結果A， B， C 為例（被遮擋點為D 點） ， 由式（1） 可得4 個靶標點的投影點TA′，TB′，TC′，TD′的理論坐標，各個投影點之間的理論距離為LTA′ _TB′，LTA′ _TC′，LTA′ _TD′，LTB′ _TC′，LTB′ _TD′，LTC′ _TD′。提取無遮擋的3 個靶標點在相機成像平面中的像素坐標，分別為（μA，vA），（μB，vB），（μC，vC）。被遮擋的點的像素坐標為（μD，vD），則通過幾何比例關系可得

由式（1）?式（3）可得D點的像素坐標（μD，vD）。結合圖像中已知的3 個光斑的像素坐標，使用PNP算法求得光學靶標中心點Ot 的空間坐標，實現對光學靶標的定位測量。

2.4.2 2 個靶標點被遮擋時光斑像素坐標推算

如果光學靶標中的2 個靶標點被遮擋，則在下一幀圖像中的輔助矩形區(qū)域框內僅能找到2 個光斑。對檢測到的2 個光斑進行標記，標記結果為A，B 或A，D 或B，C 或C，D 4 種情況。

以光斑標記結果A， B 為例（被遮擋點為C，D 點），通過式（1）求得4 個靶標點的投影點TA′，TB′，TC′，TD′的理論坐標，則被遮擋的C 點的像素坐標通過幾何比例關系求得。

由式（1） 和式（4） 可得像素坐標的2 個解（μ′C，v′C），（μ\"C，v\"C）。計算這2 個解距O3（μO3，vO3）的歐氏距離，歐氏距離最小值對應點的坐標為C 點的像素坐標，如圖5 所示。

同理，被遮擋的D 點像素坐標通過幾何比例關系求得。

由式（1）和式（5）可得D 點的像素坐標有2 個解（μ′D，v′D），（μ\"D，v\"D）。計算這2 個解距O4（μO4，vO4）的歐氏距離，歐氏距離最小值對應點的坐標為D 點的像素坐標。

3 掘進機定位試驗

為驗證光學靶標被遮擋情況下掘進機定位的精確性和穩(wěn)定性，在山東能源集團新巨龍煤礦11302回采工作面搭建井下試驗平臺，掘進巷道方向為北偏東209.355°。位姿測量裝置安裝在EBZ260 掘進機機尾的正上方，光學靶標朝向機尾方向，靶面尺寸為400 mm×360 mm。激光導引裝置懸掛在后端巷道頂板上。通過人工調整使得激光導引裝置懸掛方向與巷道掘進方向一致，此時相機坐標系與巷道坐標系一致。測試前通過試驗標定出慣導坐標系與掘進機機身坐標系的安裝偏角，以慣導系統(tǒng)輸出的姿態(tài)角（含標定的安裝偏角）與地測部門給出的巷道方向之差作為掘進機相對于巷道坐標系的航向偏角、俯仰角和橫滾角，同時也是光學靶標坐標系相對于相機坐標系的航向偏角、俯仰角和橫滾角。

掘進機組合式導航定位系統(tǒng)正常工作后，按照系統(tǒng)提供的掘進機位置坐標和姿態(tài)信息，將掘進機駕駛到距離激光導引裝置15，25，40，60 m 的測試點來采集光靶圖像。試驗過程中，用手對相應的靶標點進行遮擋，分為隨機遮擋1 個靶標點和遮擋同側2 個靶標點2 種情況。為便于比較，每個測試點下掘進機的俯仰角、橫滾角、航向偏角盡量調整為固定值，以沒有遮擋情況下解算得到的定位結果作為基準值，并與不同遮擋情況下定位結果進行對比，結果見表1。其中x，y，z 分別為沿巷道寬度方向、巷道掘進方向、巷道高度方向的定位結果，Δx，Δy，Δz 分別為沿巷道寬度方向、巷道掘進方向、巷道高度方向的定位誤差（測量值與基準值之差）。

從表1 可看出：在靶標點被遮擋時，采用本文方法得到的沿巷道寬度方向、巷道高度方向最大定位誤差分別為1，4 mm；沿巷道掘進方向最大定位誤差在掘進機距激光導引裝置5，25，40，60 m 時分別為35，34，62，69 mm，試驗結果滿足掘進機定位實際需求（沿巷道寬度方向、巷道高度方向的定位誤差影響截割斷面的控制精度，通常要求小于50 mm；沿巷道掘進方向的定位誤差影響進尺精度，通常要求小于300 mm[21]）。

4 結論

1） 在靶標點無遮擋情況下采集圖像，將圖像中光斑構造成輔助矩形區(qū)域框。當下一幀圖像中部分靶標點被遮擋時，通過比較無遮擋靶標點的成像光斑與輔助矩形區(qū)域框頂點的歐氏距離，對無遮擋靶標點的成像光斑進行標記和匹配，進而確定被遮擋靶標點。根據已知靶標幾何尺寸和慣導系統(tǒng)提供的靶標姿態(tài)信息，推導出投影后的靶標點與成像光斑的對應關系，進而求解出被遮擋靶標點對應的光斑像素坐標。

2） 在煤礦井下開展了掘進機定位試驗，結果表明：光學靶標被遮擋情況下，通過推算被遮擋靶標點對應的光斑像素坐標，可以解決掘進機定位中斷問題，保證了掘進機定位的實時性，且定位誤差滿足掘進機實際定位需求。

參考文獻（References）：

[ 1 ]王步康. 煤礦巷道掘進技術與裝備的現狀及趨勢分析[J]. 煤炭科學技術，2020，48（11）：1-11.

WANG Bukang. Current status and trend analysis ofroadway driving technology and equipment in coalmine[J]. Coal Science and Technology， 2020， 48（11） ：1-11.

[ 2 ]胡興濤，朱濤，蘇繼敏，等. 煤礦巷道智能化掘進感知關鍵技術[J]. 煤炭學報，2021，46（7）：2123-2135.

HU Xingtao，ZHU Tao，SU Jimin，et al. Key technologyof intelligent drivage perception in coal mineroadway[J]. Journal of China Coal Society， 2021，46（7）：2123-2135.

[ 3 ]劉送永，張德義. 巷道掘進機智能化技術研究現狀及展望[J]. 工礦自動化，2019，45（10）：23-28.

LIU Songyong， ZHANG Deyi. Research status andprospect of intelligentization technology ofroadheader[J]. Industry and Mine Automation， 2019，45（10）：23-28.

[ 4 ]張國喜. 懸臂式掘進機慣性測量系統(tǒng)的改進與試驗[J]. 江西煤炭科技，2021（2）：238-240.

ZHANG Guoxi. Transformation and experiment oninertial measurement system in cantileverroadheader[J]. Jiangxi Coal Science amp; Technology，2021（2）：238-240.

[ 5 ]田原. 懸臂式掘進機視覺定位方法研究[J]. 礦山機械，2019，47（3）：8-12.

TIAN Yuan. Research on vision positioning method forboom-type roadheader[J]. Mining amp; ProcessingEquipment，2019，47（3）：8-12.

[ 6 ]劉超，符世琛，成龍，等. 基于TSOA 定位原理混合算法的掘進機位姿檢測方法[J]. 煤炭學報， 2019，44（4）：1255-1264.

LIU Chao， FU Shichen， CHENG Long， et al. Posedetection method based on hybrid algorithm of TSOApositioning principle for roadheader[J]. Journal of ChinaCoal Society，2019，44（4）：1255-1264.

[ 7 ]石勇. 基于三維激光雷達的掘進機實時位姿糾偏系統(tǒng)[J]. 煤礦機械，2023，44（5）：64-66.

SHI Yong. Real-time pose correction system ofroadheader based on three-dimensional lidar[J]. CoalMine Machinery，2023，44（5）：64-66.

[ 8 ]薛光輝，張云飛，候稱心，等. 基于激光靶向跟蹤的掘進機位姿測量方法[J]. 礦業(yè)科學學報， 2020， 5（4） ：416-422.

XUE Guanghui， ZHANG Yunfei， HOU Chenxin， et al.Measurement of roadheader position and posture basedon laser target tracking[J]. Journal of Mining ScienceAnd Technology，2020，5（4）：416-422.

[ 9 ]楊文娟，張旭輝，馬宏偉，等. 懸臂式掘進機機身及截割頭位姿視覺測量系統(tǒng)研究[J]. 煤炭科學技術，2019，47（6）：50-57.

YANG Wenjuan， ZHANG Xuhui， MA Hongwei， et al.Research on position and posture measurement system ofbody and cutting head for boom-type roadheader basedon machine vision[J]. Coal Science and Technology，2019，47（6）：50-57.

[10]楊文娟，張旭輝，張超，等. 懸臂式掘進機器人巷道成形智能截割控制系統(tǒng)研究[J]. 工礦自動化， 2019，45（9）：40-46.

YANG Wenjuan，ZHANG Xuhui，ZHANG Chao，et al.Research on intelligent cutting control system forroadway forming of boom-type tunneling robot[J].Industry and Mine Automation，2019，45（9）：40-46.

[11]劉豪. 捷聯慣導與里程計組合的礦用掘進機自主導航定位系統(tǒng)[D]. 重慶：重慶大學，2020.

LIU Hao. An autonomous navigation system of miningTBM based on the combination of SINS and OD[D].Chongqing：Chongqing University，2020.

[12]張旭輝，劉博興，張超，等. 掘進機全站儀與捷聯慣導組合定位方法[J]. 工礦自動化，2020，46（9）：1-7.

ZHANG Xuhui， LIU Boxing， ZHANG Chao， et al.Roadheader positioning method combining total stationand strapdown inertial navigation system[J]. Industryand Mine Automation，2020，46（9）：1-7.

[13]崔玉明. 煤礦巷道掘進機視覺/慣性融合自主定位關鍵技術研究[D]. 徐州：中國礦業(yè)大學，2021.

CUI Yuming. Key technology research of visual/inertialfusion autonomous positioning for roadheader in coalmine[D]. Xuzhou： China University of Mining andTechnology，2021.

[14]YANG Wenjuan， ZHANG Xuhui， MA Hongwei， et al.Infrared LEDs-based pose estimation with undergroundcamera model for boom-type roadheader in coalmining[J]. IEEE Access，2019，7：33698-33712.

[15]雷孟宇，張旭輝，楊文娟，等. 煤礦掘進裝備視覺位姿檢測與控制研究現狀與趨勢[J]. 煤炭學報， 2021，46（增刊2）：1135-1148.

LEI Mengyu， ZHANG Xuhui， YANG Wenjuan， et al.Current status and trend of research on visual posedetection and control of heading equipment in coalmines[J]. Journal of China Coal Society， 2021，46（S2）：1135-1148.

[16]黃東，楊凌輝，羅文，等. 基于視覺/慣導的掘進機實時位姿測量方法研究[J]. 激光技術，2017，41（1）：19-23.

HUANG Dong，YANG Linghui，LUO Wen，et al. Studyon measurement method of realtime position and attitudeof roadheader based on vision/inertial navigationsystem[J]. Laser Technology，2017，41（1）：19-23.

[17]田原. 懸臂式掘進機導航技術現狀及其發(fā)展方向[J].工礦自動化，2017，43（8）：37-43.

TIAN Yuan. Present situation and development directionof navigation technology of boom-type roadheader[J].Industry and Mine Automation，2017，43（8）：37-43.

[18]田原. 基于四點式光靶的掘進機自動定位方法研究[J]. 煤炭科學技術，2018，46（12）：35-40.

TIAN Yuan. Research on automatic positioning methodof roadheader based on four point light target[J]. CoalScience and Technology，2018，46（12）：35-40.

[19]陳和，楊志浩，郭磐，等. 激光光斑中心高精度定位算法研究[J]. 北京理工大學學報，2016，36（2）：181-185.

CHEN He，YANG Zhihao，GUO Pan，et al. Research ofthe high precision laser spot center locationalgorithm[J]. Transactions of Beijing Institute ofTechnology，2016，36（2）：181-185.

[20]李道萍，楊波. 高精度光斑中心定位算法[J]. 光學儀器，2018，40（4）：20-25.

LI Daoping， YANG Bo. High-precision center locationalgorithm of light spot[J]. Optical Instrument， 2018，40（4）：20-25.

[21]GB 50213—2010 煤礦井巷工程質量驗收規(guī)范[S].GB 50213-2010 Code for acceptance of shaft sinkingand drifting of coal mine[S].

基金項目：北京市自然科學基金青年項目（4224094）。