煤礦井下多旋翼飛行器避障控制方法研究

郭愛軍，王妙云，馬宏偉，張旭輝，薛旭升，杜昱陽，張超

（1.神華神東煤炭集團技術研究院，陜西 神木 719315；2.西安科技大學 機械工程學院,陜西 西安 710054；3.中煤科工集團信息技術有限公司，陜西 西安 710054；4.陜西省礦山機電裝備智能檢測與控制重點實驗室，陜西 西安 710054）

0 引言

煤礦井下重要工作面環境和設備的日常巡檢是確保井下工作人員人身安全及設備安全運行、高效生產的重要措施[1]。傳統的人工巡檢方式勞動強度大、檢測結果容易受到巡檢人員水平差異的影響而存在很大的不確定性。隨著電池技術發展，多旋翼飛行器以其機械結構簡單，可以定點懸停和多方向飛行等特點在各行各業環境安全巡檢方面得到了廣泛應用。將多旋翼飛行器應用于煤礦巷道關鍵場所和裝備的巡檢，可以降低工人勞動強度，有效提升巡檢效率。

煤礦井下沒有全球定位系統（Global Positioning System，GPS）信號，加上多種電磁干擾的影響，飛行器在井下自主穩定飛行面臨巨大挑戰[2]。文獻[3]提出了基于激光雷達和毫米波雷達的四旋翼巡檢機器人井下巷道自主導航飛行的方案，利用5G網絡作為傳輸通道，對5G傳輸系統進行了驗證，效果較好。文獻[4]利用擴展卡爾曼濾波算法對三軸陀螺儀、加速度計和磁力計3種傳感器數據進行融合，實現了多旋翼飛行器姿態的檢測。文獻[5]基于迭代最近點對井下無人機實時位姿進行估計。要實現飛行器對井下設備的巡檢，僅僅獲得位姿信息是不夠的，還需研究飛行器路徑規劃和自主避障策略[6]。機器人技術與虛擬現實結合成為交叉學科研究熱點。文獻[7]構建了移動機器人作業虛擬場景，利用虛擬現實技術設計可視化仿真系統，通過改進Dijkstra算法得到了移動機器人在不規則矩形障礙下的避障策略，但該策略難以用于井下復雜環境。文獻[8]建立了履帶式掘進機器人運動控制模型，結合虛擬現實技術研究了非結構化環境下的局部避障策略，但該策略不能建立飛行器的運動模型。針對煤礦井下巡檢機器人自主定位和避障問題，文獻[9]研究了基于激光雷達的同步定位與地圖構建方法，提高了機器人的定位精度，但該方法難以保證多旋翼飛行器快速飛行的實時性要求。

針對上述問題，本文提出利用虛擬遠程操控技術對煤礦井下多旋翼飛行器進行避障的控制方法，通過礦山建井圖建立虛擬巷道模型，形成導航小地圖，減少移動過程中靜態障礙物數據的傳輸；考慮飛行器移動速度對避障結果的影響，提出了復合虛擬勢場（Compound Virtual Force Field，CVFF）避障控制算法，實現了復雜環境下飛行器的自主避障和遠程干預控制，為煤礦井下巡檢提供了新的思路。

1 煤礦井下多旋翼飛行器虛擬遠程操控系統

煤礦井下多旋翼飛行器虛擬遠程操控系統主要由飛行器虛擬遠程操控中心、數據交互中心和多旋翼飛行器及其所攜帶的傳感裝置組成，如圖1所示。系統將自主飛行避障和人為遠程干預相結合，實現了自主避障飛行為主、人為遠程干預為輔的控制方式，提高了復雜環境中多旋翼飛行器的巡檢效率。

圖1 煤礦井下多旋翼飛行器虛擬遠程操控系統組成Fig.1 Composition of virtual remote control system of underground coal mine multi-rotor aircraft

基于Unity3D開發平臺，依據煤礦巷道建井圖構建虛擬巷道模型；建立多旋翼飛行器控制數學模型，形成可交互的飛行器虛擬遠程操控中心。系統采用MySQL數據庫作為數據交互中心，用于存儲物理樣機上傳的飛行器位姿數據、工況環境數據和控制中心下發的控制命令等。巡檢過程中，多旋翼飛行器對自身所攜帶的多傳感器數據進行處理，獲得位置、姿態和避障結果，實現自主避障飛行。虛擬操控中心通過接收飛行器上傳的姿態數據、工況信息及視頻數據，驅動虛擬飛行器在虛擬巷道中同步運動，與井下巡檢場景實時保持一致。飛行中若遇到復雜環境或未知障礙導致飛行器自主決策慢，此時避障檢測單元根據巷道環境的變化情況，以當前的位置環境狀況，利用CVFF避障控制算法實時修正飛行軌跡。緊急情況下，遠程操控人員以虛擬仿真界面上飛行器虛擬樣機與虛擬仿真環境之間的關系為控制依據，通過遠程控制臺對井下多旋翼飛行器進行人為遠程干預。

2 煤礦井下多旋翼飛行器避障控制方法

在復雜巷道內巡檢時，煤礦多旋翼飛行器不僅要為自主控制規劃全局路徑，還要在遇到動態障礙物時完成局部避障路徑規劃和人為遠程干預[10]。通過數字化虛擬巷道可以提前獲得封閉環境內的路徑信息，生成導航地圖，建立已知的靜態環境模型，減少運動過程中飛行器對環境感知建模的任務量，可以提高虛擬遠程操控的運行效率。備飛階段，可以通過提前建立的虛擬巷道規劃飛行器的移動路徑。在移動過程中，飛行器通過自身攜帶的傳感設備檢測移動方向的動態障礙物信息。虛擬遠程操控系統根據障礙物數據將動態障礙物實時重建于初始虛擬巷道模型中，對虛擬環境狀態進行實時更新，為飛行器局部避障控制提供可靠的環境依據。虛擬遠程操控系統通過讀取障礙物與飛行器的位置數據和移動速度信息，采用CVFF避障控制算法進行避障路徑規劃，如果檢測到前方障礙物對飛行器移動產生巨大威脅時，遠程操控人員可根據規劃的避障路徑對飛行器進行遠程干預，具體的避障控制流程如圖2所示。

圖2 復雜環境下多旋翼飛行器避障控制流程Fig.2 Obstacle avoidance control flow for multi-rotor aircraft in complex environment

2.1 虛擬巷道模型建立

煤礦多旋翼飛行器在復雜井下巷道飛行的全局路徑規劃基于礦井初始地圖實現。根據煤礦巷道初始地圖庫信息在虛擬遠程操控系統中建立初始巷道模型及全局導航地圖，可提前獲得飛行器移動過程中已知的靜態障礙物信息，在全局路徑中為飛行器規劃初始導航路線。將初始巷道模型導入Unity3D平臺[11]，通過坐標系變換和模型渲染，形成井下巷道虛擬仿真場景。全局虛擬巷道模型如圖3所示，局部虛擬巷道模型如圖4所示。

圖4 局部虛擬巷道模型Fig.4 The local coal mine virtual roadway model

初始巷道模型建立后，在巷道模型正上方設置一個Minimap Camera相機實時拍攝模型的全局路徑。在Minimap Camera相機的檢視面板中設置Minimap Render Texture，將獲得的全局路徑生成一張用戶界面（User Interface，UI）圖片放置于監測界面的右上角，作為導航小地圖，如圖3右上角所示。導航小地圖在X方向的縮放比例和Y方向的縮放比例fY分別為

圖3 全局虛擬巷道模型Fig.3 The global coal mine virtual roadway model

式中：(r，u)為飛行器在巷道模型中的坐標；(R，U)為巷道模型尺寸。

在小地圖上同步飛行器在巷道的位置：

式中：(r′，u′)為飛行器同步在導航小地圖上的坐標；(R′，U′)為導航小地圖尺寸。

在獲得初始巷道模型和導航小地圖之后，可通過Unity3D平臺NavMesh導航網格尋路模塊將復雜的三維環境結構劃分成帶有尋路導航信息的網格。調用Navigation Static模塊，在初始巷道模型中將移動路線自動生成導航網格，Navigation視圖的Bake命令將劃分好的可移動區域導航網格以藍色柵格形式顯示出來，如圖5所示。在規劃好的路徑中，通過編輯C#腳本完成目標位置TargetObject的定義，調用NavMesh Agent命令將多旋翼飛行器虛擬模型設定為移動對象，在Unity3D平臺中獲得全局地圖，為飛行器自主飛行提供導航路徑。

圖5 全局地圖內生成的導航網格Fig.5 Navigation grid on theglobal map

2.2 CVFF避障控制算法

虛擬勢場（Virtual Force Field, VFF）算法是移動機器人避障導航技術中最常用的算法之一，該算法實質是使用柵格法來表示環境，在虛擬勢場中引入環境對多旋翼飛行器的影響[12]。VFF算法的基本思想是考慮了障礙物柵格對飛行器的排斥力和目標點對飛行器的吸引力的共同作用，從而控制飛行器按照避障規劃路徑飛行[13]。

CVFF避障控制算法在VFF算法的基礎上融入了速度勢場，在計算過程中引入飛行器與障礙物、目標點之間的相對速度，提高了飛行器對動態障礙物的感知效率和精度。該算法采用柵格法將避障檢測單元采集到的障礙物信息局部環境分成若干個柵格，根據障礙物的位置與形狀，每個柵格將會對飛行器產生相應大小的斥力作用，每個矩形柵格都有一個累計值（Certainty Value，CV），表示此方格內存在障礙物的可能性大小。定義Ci,j為第 (i,j)個柵格內存在障礙物的可信度，Ci,j越大，表示此柵格存在障礙物的可能性越高，沒有檢測到障礙物的范圍內Ci,j=0， Ci,j通過飛行器所攜帶的傳感設備對空間中障礙物的檢測結果進行刷新。

在位置勢場和速度勢場共同作用下，飛行器在復合虛擬勢場中的受力情況如圖6所示，其中，Frep為飛行器在復合勢場中受到的障礙物柵格的排斥力，Fatt為飛行器在復合勢場中受到的目標點的引力，飛行器在虛擬復合勢場中所受合力為F。

圖6 多旋翼飛行器在復合虛擬勢場中的受力Fig.6 Theforce of multi-rotor aircraft in compound virtual forcefield

2.2.1 斥力勢場強度

在飛行過程中，每個小柵格(i,j)對飛行器作用一個虛擬斥力Fi,j，斥力的大小與柵格的可信度 Ci,j成正比，與飛行器到障礙物之間的距離di,j的平方成反比。

假設飛行器的坐標為(x0,y0)，障礙物柵格的坐標為(xz,yz)，目標點的坐標為(xa,ya)，飛行器的飛行速度為Vrobot，障礙物的運動速度為 Vobject，則飛行器在復合勢場中受到的障礙物柵格的排斥力為

式中：Frepp為飛行器在位置勢場中受到的斥力； Frepv為飛行器在速度勢場中受到的斥力。

式中：Mp為 位置勢場的引力增益系數；分別為X方向和Y方向的單位向量。

式中：Mv為 速度勢場中的斥力增益系數；Vb為飛行器相對于障礙物的運動速度；θ為飛行器與障礙物相對位置矢量與相對運動速度矢量的夾角；β 為Vb與X軸的夾角。

2.2.2 引力勢場強度

飛行器在復合勢場中受到目標點的引力為

式中： Fattp為飛行器在位置勢場中受到的引力； Fattv為飛行器在速度勢場中受到的引力。

式中：Kp為 位置勢場的引力增益系數；da為飛行器與目標點之間的相對距離。

式中：Kv為 速度勢場引力增益系數；Vg為飛行器相對于目標點的運動速度；α為 Vg與X軸的夾角。

因此，飛行器在虛擬復合勢場中所受合力為

假設飛行器運動步長為l0，飛行器在虛擬復合勢場中所受合力在X軸，Y軸上的分量分別為FX，FY， 時刻飛行器的坐標為 ，則該飛行器在 時刻的坐標為 ，其計算公式為t(xt，yt) t+1(xt+1，yt+1)

2.2.3 算法的設計與實現

CVFF避障控制算法流程如圖7所示。

圖7 CVFF避障控制算法流程Fig.7 Compound virtual force field obstacle avoidance control algorithm flow

CVFF避障控制算法具體實現步驟如下：

（1）初始化參數。獲取飛行器、障礙物和目標點的位置坐標，設定位置勢場和速度的相關參數。

（2）調用Compute_Angle模塊分別計算飛行器與障礙物和目標位置之間的夾角。

（3）分別調用Compute_Attract模塊和Compute_Repulsion模塊，計算引力和斥力在X軸和Y軸的分量。

（4）計算飛行器在下一時刻的位置坐標，并進行記錄。

（5）判斷飛行器是否到達目標位置，如果到達指定位置則跳出程序，否則回到步驟（2），進入下一次循環。

3 仿真實驗與分析

3.1 靜態障礙物避障路徑仿真分析

為了驗證煤礦井下多旋翼飛行器避障控制方法的效果，在Matlab仿真平臺中構建10×10的環境感知柵格，飛行器在避障過程中的相關參數設置[14]見表1。

表1 仿真環境參數Table 1 Simulation environment parameters

實驗中設置了p0=0.5和p0=1，飛行器的起始坐標為（0，0），目標位置為（10，10），比較靜態環境下VFF算法和CVFF避障控制算法的避障效果，其避障控制仿真結果如圖8?圖11所示。VFF算法與CVFF避障控制算法仿真結果性能比較見表2和表3。

圖8 p0=0.5時VFF算法仿真結果Fig.8 Simulation resultsof virtual force field algorithm when p0= 0.5

從圖8?圖11和表2、表3可看出，當障礙物影響距離p0分別為0.5和1時，在相同的避障環境中，CVFF避障控制算法迭代次數更少，計算效率更高，且得到的避障軌跡長度更優，避障效果更高效。VFF算法在障礙物比較密集的區域，避障效果不理想，當避障影響距離較小時容易與障礙物之間發生碰撞。通過比較圖9與圖11的軌跡路線，表明了障礙物影響距離對避障路線的影響較大，在本文搭建的仿真環境中，障礙物影響距離為0.5個單位時，軌跡規劃結果最理想。

圖9 p0=0.5時CVFF避障控制算法仿真結果Fig.9 Simulation results of compound virtual forcefield obstacle avoidance control algorithm when p0=0.5

圖10 p0=1時VFF算法仿真結果Fig.10 Simulation results of virtual force field algorithm when p0=1

圖11 p0=1時CVFF避障控制算法仿真結果Fig.11 Simulation resultsof compound virtual force field obstacle avoidancecontrol algorithm when p0=1

表2 p0=0.5時2種算法性能比較Table 2 Performancecomparison of two algorithmswhen p0=0.5

3.2 動態障礙物避障路徑仿真分析

利用CVFF避障控制算法對動態障礙物的避障效果進行仿真實驗。首先設置了100×100的柵格仿真環境，飛行器起點坐標為（0，0），運動速度Vrobot=(1.5，1.5)，終點坐標為（100，100）。仿真過程設置了2個動態障礙物，第1個障礙物的起點坐標為（33，88），運動速度Vobject1=(0.50，?3.75)，第2個障礙物的起點坐標為（65，7），運動速度Vobject2=(0.2，2.0)。

隨機選取了6個時刻記錄飛行器與障礙物的運動情況，如圖12所示，可看出飛行器成功避開了2個動態障礙物。

圖12 多旋翼飛行器動態避障過程Fig.12 Obstacle avoidance processof multi-rotor aircraft

運動過程中飛行器與第1個動態障礙物之間的相對位置變換情況如圖13所示。在t =0時，飛行器向著第1個障礙物方向飛行，兩者之間距離不斷減小，飛行器自動調整飛行方向；在t = 12 s時，兩者距離為5 dm，此時剛好沒有發生碰撞，隨后飛行器朝遠離第1個障礙物方向飛行，兩者距離逐漸增大。

圖13 飛行器與第1個障礙物之間的距離Fig.13 Distance between the aircraft and the first obstacle

飛行器與第2個障礙物的相對位置變換情況如圖14所示，在t = 0時，飛行器與第2個障礙物的間距為65 dm，不會發生碰撞，飛行器向著第2個障礙物方向飛行，兩者之間距離不斷減小，在這一過程中飛行器自動調整飛行方向；在t = 31 s時，兩者距離為8 dm，沒有發生碰撞，隨后飛行器朝遠離第2個障礙物方向飛行，兩者距離逐漸增大。

圖14 飛行器與第2個障礙物之間的距離Fig.14 Distancebetween the aircraft and thesecond obstacle

以上分析表明，本文提出的避障控制方法不僅能使飛行器在靜態環境中實現穩定避障，在動態威脅下也能實現精確避障飛行。

4 結論

（1）提出了一種利用虛擬遠程操控技術對煤礦井下多旋翼飛行器進行避障的控制方法，根據煤礦巷道初始信息建立了虛擬巷道模型和全局導航地圖，減少移動過程中靜態障礙物數據的傳輸；通過飛行器攜帶的傳感設備檢測移動方向的動態障礙物信息，采用CVFF避障控制算法進行避障路徑規劃。

（2）提出了一種CVFF避障控制算法，在VFF的基礎上引入飛行器與障礙物、目標點之間的相對速度，提高了飛行器對動態障礙物的感知效率和精度。

（3）從靜態和動態障礙物避障路徑2個方面對CVFF避障控制算法進行仿真驗證，結果表明：靜態情況下，相比VFF算法，CVFF避障控制算法在減少了迭代次數的同時，也縮短了飛行器的軌跡長度；動態情況下，飛行器成功避開了提前設定的2個動態障礙物，順利到達設定目標點，驗證了采用CVFF算法的多旋翼飛行器避障控制方法的有效性。