基于三維點(diǎn)云處理的輔助作業(yè)機(jī)器人位姿控制方法

摘 要：煤礦井下輔助作業(yè)機(jī)器人位姿控制是提高機(jī)器人作業(yè)效率和質(zhì)量的重要手段，目前控制效果不佳，機(jī)器人控制誤差較大，因此本文提出基于三維點(diǎn)云處理的煤礦井下輔助作業(yè)機(jī)器人位姿控制方法。對(duì)煤礦井下進(jìn)行三維激光掃描，獲取井下三維點(diǎn)云數(shù)據(jù)，進(jìn)行三維點(diǎn)云處理，搭建井下空間三維場(chǎng)景模型，提供識(shí)別障礙物和作業(yè)目標(biāo)，確定機(jī)器人位姿調(diào)控量，基于三維點(diǎn)云處理對(duì)煤礦井下輔助作業(yè)機(jī)器人進(jìn)行位姿控制。試驗(yàn)證明，當(dāng)采用本文方法時(shí)，機(jī)器人避障效果較好，位置跟蹤誤差較低，機(jī)器人作業(yè)位置與目標(biāo)位置誤差僅為0.1 m，可以對(duì)煤礦井下輔助作業(yè)機(jī)器人位姿進(jìn)行精準(zhǔn)控制。

關(guān)鍵詞：三維點(diǎn)云處理；輔助作業(yè)機(jī)器人；位姿控制；三維激光掃描；障礙物；作業(yè)目標(biāo)

中圖分類號(hào)：TP 242" " " " " 文獻(xiàn)標(biāo)志碼：A

隨著煤炭工業(yè)快速發(fā)展，煤礦井下作業(yè)效率和安全性成為行業(yè)內(nèi)關(guān)注的焦點(diǎn)。為了提升井下作業(yè)的效率和安全性，在煤礦生產(chǎn)中采用智能化設(shè)備。煤礦井下輔助作業(yè)機(jī)器人是一種新型的智能化設(shè)備，其具有自主導(dǎo)航、作業(yè)執(zhí)行以及信息感知等功能，其可以通過傳感器和定位系統(tǒng)獲取周圍環(huán)境的信息，利用計(jì)算機(jī)視覺和人工智能技術(shù)進(jìn)行識(shí)別和判斷。當(dāng)執(zhí)行作業(yè)任務(wù)時(shí)，機(jī)器人可以利用預(yù)設(shè)路徑或者自主探索方式進(jìn)行移動(dòng)，完成各種井下作業(yè)任務(wù)，例如鉆孔、搬運(yùn)、噴漿和巡檢等，節(jié)約生產(chǎn)成本，降低事故發(fā)生的概率，保障井下作業(yè)安全。機(jī)器人還可以為礦工提供更好的工作環(huán)境，減輕勞動(dòng)強(qiáng)度，提高工作效率。目前，煤礦井下輔助作業(yè)機(jī)器人在采礦行業(yè)中廣泛應(yīng)用，位姿控制是煤礦井下輔助作業(yè)機(jī)器人高效、安全作業(yè)的關(guān)鍵技術(shù)之一。

1 井下作業(yè)空間三維激光掃描

煤礦井下作業(yè)空間狹窄，環(huán)境惡劣，巷道崎嶇不平，障礙物較多，光線條件較差，提高了機(jī)器人定位建圖難度。因此，本文采用三維點(diǎn)云處理技術(shù)虛擬化井下作業(yè)空間場(chǎng)景，識(shí)別場(chǎng)景內(nèi)機(jī)器人作業(yè)目標(biāo)和周圍障礙物，為機(jī)器人位姿控制提供依據(jù)。為實(shí)現(xiàn)該目標(biāo)，需要對(duì)煤礦井下作業(yè)空間進(jìn)行三維激光掃描，獲取井下作業(yè)空間的三維點(diǎn)云數(shù)據(jù)[1]。將型號(hào)為OYTAS-A7F7的三維激光掃描儀安裝在機(jī)器人上方，探頭與地面平行，采用串并聯(lián)的方式將其接入機(jī)器人電源總線上，使兩者同步運(yùn)行。根據(jù)實(shí)際情況設(shè)定三維激光掃描儀掃描范圍、周期以及頻率等技術(shù)方面參數(shù)。在掃描過程中，激光測(cè)距儀發(fā)射一束激光脈沖信號(hào)，當(dāng)接觸物體表面時(shí)，信號(hào)發(fā)生漫反射并沿著原路徑反向傳導(dǎo)至信號(hào)接收器，可以得到機(jī)器人與掃描點(diǎn)之間的距離，如公式（1）所示[2]。

（1）

式中：h為激光掃描儀與掃描點(diǎn)之間的距離，即機(jī)器人與掃描點(diǎn)之間的距離；v為激光脈沖信號(hào)傳播速度；t1、t2分別為激光脈沖信號(hào)發(fā)射時(shí)間與反射時(shí)間。電機(jī)驅(qū)動(dòng)三維激光掃描儀上部轉(zhuǎn)動(dòng)，根據(jù)掃描儀豎向鏡垂直方向與水平方向轉(zhuǎn)過的角度，得到三維激光掃描儀觀測(cè)角度值，將測(cè)量值映射至掃描儀坐標(biāo)系中，得到掃描點(diǎn)的坐標(biāo)，如公式（2）所示[3]。

（2）

式中：Q為掃描儀坐標(biāo)系；X、Y和Z分別為掃描點(diǎn)在三維激光掃描儀中的橫坐標(biāo)、縱坐標(biāo)和斜坐標(biāo)；α為水平方向觀測(cè)角度；β為垂直方向觀測(cè)角度。采集煤礦井下作業(yè)空間三維點(diǎn)云數(shù)據(jù)，上傳至計(jì)算機(jī)，用于三維點(diǎn)云處理。

2 基于三維點(diǎn)云處理的作業(yè)三維場(chǎng)景搭建

處理三維激光掃描得到的井下作業(yè)空間三維點(diǎn)云數(shù)據(jù)，搭建井下作業(yè)三維虛擬場(chǎng)景。將采集的三維點(diǎn)云數(shù)據(jù)中的重復(fù)部分刪除，對(duì)異常和缺失數(shù)據(jù)進(jìn)行插值處理，保證三維點(diǎn)云數(shù)據(jù)完整性[4]。當(dāng)三維激光掃描時(shí)會(huì)受井下作業(yè)噪聲干擾，采集的點(diǎn)云數(shù)據(jù)存在噪聲。為了保證控制精度，采用均值濾波法對(duì)三維點(diǎn)云濾波進(jìn)行去噪處理，如公式（3）所示。

（3）

式中：X*、Y*和Z*為三維點(diǎn)云坐標(biāo)；Qt-1、Qt+1分別為前一個(gè)時(shí)刻和下一個(gè)時(shí)刻掃描點(diǎn)坐標(biāo)值。根據(jù)均值對(duì)三維點(diǎn)云數(shù)據(jù)中的離散點(diǎn)進(jìn)行平滑處理，去除原始點(diǎn)云數(shù)據(jù)中的噪聲分量。將處理好的三維點(diǎn)云數(shù)據(jù)上傳至UFH三維建模軟件中，利用轉(zhuǎn)換矩陣將掃描儀坐標(biāo)系轉(zhuǎn)換為大地坐標(biāo)系，對(duì)三維點(diǎn)云數(shù)據(jù)進(jìn)行拼接處理，如公式（4）所示。

（4）

式中：S（x，y，z）為大地坐標(biāo)系中掃描點(diǎn)位置坐標(biāo)；x為水平方向坐標(biāo)；y為豎直方向坐標(biāo)；z為垂直于兩者方向的坐標(biāo)；E為旋轉(zhuǎn)矩陣；F為平移矩陣。將三維點(diǎn)云數(shù)據(jù)映射至大地坐標(biāo)系中，搭建煤礦井下輔助作業(yè)空間三維場(chǎng)景模型，為后續(xù)機(jī)器人位姿控制奠定基礎(chǔ)。

3 作業(yè)目標(biāo)定位及機(jī)器人位姿控制

在上述基礎(chǔ)上識(shí)別三維空間場(chǎng)景的障礙物，在三維空間場(chǎng)景中，如果掃描點(diǎn)縱向坐標(biāo)值高于機(jī)器人位置縱向坐標(biāo)值，則視該點(diǎn)為障礙物，如公式（5）所示。

（5）

式中：g為障礙物識(shí)別結(jié)果；1為掃描點(diǎn)障礙物；0為掃描點(diǎn)無障礙物；a為機(jī)器人縱坐標(biāo)。利用公式（5）識(shí)別得到三維場(chǎng)景中的障礙物，并將機(jī)器人作業(yè)目標(biāo)標(biāo)記在大地坐標(biāo)系中，在未識(shí)別障礙物的情況下，機(jī)器人向作業(yè)目標(biāo)成直線移動(dòng)；當(dāng)識(shí)別到障礙物時(shí)，根據(jù)機(jī)器人當(dāng)前位姿以及障礙物位置，確定機(jī)器人位姿調(diào)控量。機(jī)器人運(yùn)動(dòng)學(xué)模型如公式（6）所示。

（6）

式中：b為機(jī)器人橫坐標(biāo)；γ為機(jī)器人姿態(tài)角。

機(jī)器人動(dòng)力學(xué)模型如公式（7）所示。

（7）

式中：M為機(jī)器人移動(dòng)慣性，即加速度；C為機(jī)器人速度與位置有關(guān)的向心力；μ為機(jī)器人約束反力。利用加速度傳感器進(jìn)行測(cè)量，得到機(jī)器人的移動(dòng)慣性，為了改變機(jī)器人的運(yùn)動(dòng)方向，須改變機(jī)器人運(yùn)動(dòng)模型中的姿態(tài)角。機(jī)器人姿態(tài)控制可以改變機(jī)器人姿態(tài)角，越過障礙物向目標(biāo)位置移動(dòng)，因此其位姿控制量的計(jì)算方法如公式（8）所示。

（8）

式中：υγ為煤礦井下輔助作業(yè)機(jī)器人位姿控制量；δ為機(jī)器人作業(yè)目標(biāo)位置；S*（x，y，z）為機(jī)器人前方障礙物坐標(biāo)。對(duì)機(jī)器人進(jìn)行位姿調(diào)控，改變機(jī)器人移動(dòng)方向，越過障礙物。將以上計(jì)算的控制量發(fā)送至控制器中，利用控制器對(duì)機(jī)器人位姿進(jìn)行調(diào)控，對(duì)基于三維點(diǎn)云處理的煤礦井下輔助作業(yè)機(jī)器人進(jìn)行位姿控制。

4 試驗(yàn)論證

4.1 試驗(yàn)工況

煤礦井下采掘工作面風(fēng)流溫度為28 ℃～30 ℃，作業(yè)地點(diǎn)的風(fēng)流速度為2.5 m/s～3.0 m/s，濕度為80%～100%，大氣壓為103 hPa～108 hPa，甲烷含量為0.1%～15.0%，井下輔助作業(yè)機(jī)器人承壓為5.5 MPa，導(dǎo)熱系數(shù)為0.02 W/（m·℃）。

4.2 試驗(yàn)準(zhǔn)備與設(shè)計(jì)

為驗(yàn)證設(shè)計(jì)方法能否應(yīng)用于煤礦井下作業(yè)環(huán)境，對(duì)煤礦井下輔助作業(yè)機(jī)器人進(jìn)行試驗(yàn)，機(jī)器人型號(hào)為KHFA-SG455，移動(dòng)速度為1.5 m/s，井下巷道寬度為5.14 m，長(zhǎng)度為1 564.52 m，利用本文方法對(duì)煤礦井下輔助作業(yè)機(jī)器人進(jìn)行位姿控制。試驗(yàn)過程為在井下輔助作業(yè)機(jī)器人上安裝1臺(tái)三維激光掃描儀，采集井下三維激光掃描數(shù)據(jù)0.26 GB，分析并處理三維激光掃描數(shù)據(jù)，搭建井下三維場(chǎng)景，控制機(jī)器人位姿，使其完成井下500 m作業(yè)任務(wù)，煤礦井下輔助作業(yè)機(jī)器人位姿作業(yè)任務(wù)如圖1所示。從圖1可以看出，在對(duì)機(jī)器人進(jìn)行位姿控制的過程中，機(jī)器人能夠繞開所有障礙物并按照規(guī)定路線向目標(biāo)移動(dòng)，說明本文方法基本可以完成煤礦井下輔助作業(yè)機(jī)器人位姿控制任務(wù)。

4.3 試驗(yàn)結(jié)果

4.3.1 煤礦井下輔助作業(yè)機(jī)器人避障效果測(cè)試

應(yīng)用設(shè)計(jì)方法后的機(jī)器人位姿作業(yè)路徑與煤礦井下輔助作業(yè)機(jī)器人位姿作業(yè)任務(wù)路徑軌跡越一致，方法避障能力越強(qiáng)。具體測(cè)試結(jié)果如圖2所示。

根據(jù)圖2可知，設(shè)計(jì)方法采用三維點(diǎn)云處理技術(shù)，利用激光雷達(dá)傳感器獲取井下環(huán)境的點(diǎn)云數(shù)據(jù)，這些數(shù)據(jù)包括豐富的空間信息和幾何特征，因此設(shè)計(jì)方法與煤礦井下輔助作業(yè)機(jī)器人位姿作業(yè)任務(wù)路徑基本一致。處理并分析這些點(diǎn)云數(shù)據(jù)，得到包括周圍障礙物的位置、形狀和大小等準(zhǔn)確的煤礦井下作業(yè)空間環(huán)境信息，構(gòu)建井下作業(yè)空間的三維模型，識(shí)別機(jī)器人作業(yè)目標(biāo)和周圍障礙物。由于避障路徑的規(guī)劃基于準(zhǔn)確的三維模型和障礙物信息，因此，當(dāng)遇到障礙物時(shí)，煤礦井下輔助作業(yè)機(jī)器人能夠做出快速、準(zhǔn)確的反應(yīng)，避免與障礙物發(fā)生碰撞。

4.3.2 煤礦井下輔助作業(yè)機(jī)器人位置跟蹤誤差測(cè)試

位置跟蹤誤差測(cè)試能夠直接反映煤礦井下輔助作業(yè)機(jī)器人的定位精度，對(duì)評(píng)估機(jī)器人的導(dǎo)航和定位能力十分重要。利用EFly智能航測(cè)軟件，采用自動(dòng)數(shù)據(jù)采集終端采集設(shè)計(jì)方法的煤礦井下輔助作業(yè)機(jī)器人位置信息。獲取機(jī)器人在X軸、Y軸和Z軸的平均位置跟蹤誤差，誤差越低，煤礦井下輔助作業(yè)機(jī)器人位姿控制效果越好。具體測(cè)試結(jié)果如圖3所示。

由圖3可知，采用均值濾波法對(duì)三維點(diǎn)云數(shù)據(jù)進(jìn)行濾波去噪處理，并對(duì)離散點(diǎn)進(jìn)行平滑處理，有效去除原始數(shù)據(jù)中的噪聲分量，減少數(shù)據(jù)中的隨機(jī)誤差和異常值，點(diǎn)云數(shù)據(jù)更平滑和連續(xù)。因此，利用本文方法控制的礦井下輔助作業(yè)機(jī)器人在X軸、Y軸和Z軸的平均位置跟蹤誤差均較低，三維軸向的誤差波動(dòng)較平緩。將處理好的三維點(diǎn)云數(shù)據(jù)上傳至UFH三維建模軟件中，及時(shí)調(diào)整機(jī)器人的路徑和姿態(tài)，完成動(dòng)態(tài)避障。利用轉(zhuǎn)換矩陣將掃描儀坐標(biāo)系轉(zhuǎn)換為大地坐標(biāo)系，對(duì)三維點(diǎn)云數(shù)據(jù)進(jìn)行拼接處理，使機(jī)器人在井下作業(yè)過程中的位置數(shù)據(jù)與實(shí)際環(huán)境坐標(biāo)系一致，提高了機(jī)器人位姿控制精度。

4.3.3 煤礦井下輔助作業(yè)機(jī)器人位姿控制精度測(cè)試

極值搜索算法是一種自適應(yīng)控制方法，其可以在時(shí)變環(huán)境中尋找系統(tǒng)的最佳性能點(diǎn)。優(yōu)化機(jī)器人位姿參數(shù)，利用極值搜索算法取得最佳作業(yè)效果。模糊PID控制算法是一種結(jié)合了模糊邏輯和傳統(tǒng)PID控制的方法，利用模糊化處理系統(tǒng)輸入和輸出，改善傳統(tǒng)PID控制器對(duì)非線性、時(shí)變系統(tǒng)的控制性能。模糊PID控制算法根據(jù)機(jī)器人當(dāng)前位姿誤差和誤差變化率動(dòng)態(tài)調(diào)整PID控制參數(shù)，提升機(jī)器人在復(fù)雜環(huán)境中的位姿控制性能。為了驗(yàn)證方法效果，試驗(yàn)選擇極值搜索算法（傳統(tǒng)方法一）和模糊PID控制算法（傳統(tǒng)方法二）與本文方法進(jìn)行對(duì)比。試驗(yàn)統(tǒng)計(jì)了7次機(jī)器人作業(yè)任務(wù)中目標(biāo)點(diǎn)位置與實(shí)際位置的數(shù)據(jù)，機(jī)器人作業(yè)位置與目標(biāo)位置越近，機(jī)器人位姿控制精度越高，效果越好，統(tǒng)計(jì)數(shù)據(jù)見表1。

根據(jù)表1的數(shù)據(jù)得出結(jié)論，應(yīng)用本文方法，機(jī)器人作業(yè)位置與目標(biāo)位置最大誤差僅為0.1 m，傳統(tǒng)方法一的最大誤差為1.2 m，傳統(tǒng)方法二的最大誤差為1.8 m，大于本文方法的最大誤差。試驗(yàn)結(jié)果說明采用本文方法，機(jī)器人位姿控制精度較高。

5 結(jié)語

本文提出一種基于三維點(diǎn)云處理的煤礦井下輔助作業(yè)機(jī)器人位姿控制方法。經(jīng)過試驗(yàn)驗(yàn)證，本文方法能夠?qū)γ旱V井下輔助作業(yè)機(jī)器人進(jìn)行精確定位和姿態(tài)控制，提高機(jī)器人在井下作業(yè)的效率，保障安全。該方法還可以為其他類似的機(jī)器人系統(tǒng)提供一種有效的位姿控制方法。該方法在實(shí)際應(yīng)用中仍然存在一些挑戰(zhàn)。例如，井下環(huán)境的復(fù)雜性和動(dòng)態(tài)性可能導(dǎo)致點(diǎn)云數(shù)據(jù)的獲取和處理難度較大；機(jī)器人在井下的運(yùn)動(dòng)受到限制和約束，需要更精確的軌跡規(guī)劃和姿態(tài)控制。為了解決這些問題，從以下2個(gè)方面進(jìn)行研究。1）進(jìn)一步研究點(diǎn)云數(shù)據(jù)的獲取和處理技術(shù)，提高數(shù)據(jù)的準(zhǔn)確性和實(shí)時(shí)性。2）探索更有效的位姿估計(jì)方法，提高機(jī)器人的定位和姿態(tài)控制精度。

參考文獻(xiàn)

[1]趙喜江，楊志強(qiáng)，張宇.煤礦豎井井筒三維激光掃描的誤差分析與控制[J].黑龍江科技大學(xué)學(xué)報(bào)，2022，32（4）：6.

[2]毛成林，于瑞強(qiáng)，宋愛國(guó).基于各向異性空間投影的地鐵列車底部巡檢機(jī)器人避障研究[J].機(jī)械與電子， 2023，41（8）：76-80.

[3]李鳴昊，王輝，伍思?xì)g，等.考慮執(zhí)行器最優(yōu)分配的空間站艙內(nèi)機(jī)器人位姿耦合控制[J].宇航學(xué)報(bào)，2023，44（7）：1073-1083.

[4]韓勇，霍迎秋，費(fèi)攀鋒.基于模糊PID的電力鐵塔攀爬機(jī)器人末端位姿控制研究[J].工業(yè)儀表與自動(dòng)化裝置，2023（3）：80-84，87.