基于TSP理論的礦井作業機器人路徑規劃問題探究

崔伏建

（國投煤炭鄭州能源開發有限公司，河南登封，452470）

基于TSP理論的礦井作業機器人路徑規劃問題探究

崔伏建

（國投煤炭鄭州能源開發有限公司，河南登封，452470）

隨著國家對自動化作業的重視，越來越多的礦井開始引入礦井作業機器人進行煤礦的挖掘和開采，但是礦井下環境惡劣，而且大型礦井內部結構復雜，因此井下作業機器人對路徑規劃能力有很高的要求。本文引入旅行商問題的思想尋求最優運動路徑；應用圖論相關知識對礦井結構示意圖進行分析轉化，引入Dijkstra算法解決最短路徑問題，并在代價矩陣中添加角度遞增函數確定優先權，最后使用離散粒子群算法進行仿真求解，取得了較好的實驗效果。

礦井作業機器人；路徑規劃；旅行商問題；Dijkstra算法；離散粒子群算法

0 引言

煤礦礦井內部結構復雜，條件艱苦。有些作業環境不適合礦工長期作業，另外在礦難發生后，由于往往對井下的情況未知，會給救援人員帶來很大的救援困難，不能夠抵達救援地點，而機器人的發展，將會為井下作業帶來極大的方便，其中針對礦井救援機器人，路徑規劃能力是非常重要的考慮因素。機器人的路徑規劃問題主要是為了找到一條從出發點到目標點的最佳或次佳路徑。本文所研究的背景是基于復雜環境下的礦井救援機器人的路徑規劃設計，復雜礦井環境（如圖1）下的救援機器人的主要目標是機器人在盡可能短的時間內游歷盡量多的礦井節點，完成救援活動，并回到出發地點。這是一種典型的尋找最優路徑規劃的范例。這和傳統的旅行商問題（簡稱TSP）非常類似，TSP要求尋找一條經過每個頂點至少一次的權最小的閉通路。因此，本文嘗試將“復雜環境下礦井救援機器人”的路徑規劃問題轉化為標準TSP問題，并利用離散粒子群算法進行仿真求解。

1 “復雜環境下礦井救援機器人路徑規劃”向“TSP問題”的轉化

1.1 礦井節點坐標的建立

圖1 礦井結點編號對照

為了解決復雜礦井環境下救援機器人路徑規劃這個實際問題，我們首先要建立模擬礦井節點的坐標，在該坐標戲中，我們首先將各個節點視為質點，并對其進行編號得到圖1；

1.2 簡化場地圖

為了方便求解與構造哈密爾頓圖用于后續仿真分析，在不影響求解效果的前提下，將抽象的礦井復雜環境模擬分布地圖進行簡化，具體過程如下：

（1）如圖3所示，以節點19為例，機器人想要到達節點2，3必須先經過節點19，然后原路返回在經過一次節點19，所以在尋找機器人最優路徑時，我們可以先略去外圍所有像節點2、3一樣的點，即節點2、3、4、5、6、7、8、9、10、11、12、13、14、15、16、17。

圖2 簡化礦井分布圖1

（2）圖2中黑色陰影內的節點，即節點22、25、27、34并非礦井節點，也不與設計節點直接相連，但他們是救援機器人“搜索救援”時的必經節點，它們通常連接了2個以上目的節點。我們將這些節點略去，并對機器人“搜索救援”線路進行重新賦值。處理得到的簡化礦井環境分布如圖3所示：

圖3 簡化礦井分布圖2

本文所設計的相應的權值計算公式為W(23，33)=W(23，25)+W(25，33)，并以此類推。

（3）最終通過權值轉換得到的轉化圖如圖4所示：

圖4 簡化礦井分布圖3

根據權值計算公式可得權值矩陣W，并從1到14重新進行標號。簡化的過程中，記錄下節點編號的對照分布圖5。圖中，第4列節點是在問題轉化過程中省略掉的節點，它是機器人要到達的礦井節點標號，與第3列節點是對應相連的關系。圖2轉化為圖3的過程中，我們將機器人不必要經過的節點省略掉，并重新對其編號為圖中第2列。根據圖論知識繼續轉化得到最簡圖4，并對應圖3終結點重新編號，如圖中第一列所示，空白的地方為圖4省略的點，為表明其對應關系而空出。

圖5 結點編號轉換

1.3 引入Dijkstra算法建立最短距離矩陣

完成上述工作后，需要解決的問題就是找到簡化圖中任意兩點的最短距離，這樣本文的路徑規劃問題就可以轉換為TSP問題進行求解了。為此，本文引入Dijkstra矩陣算法。Dijkstra算法可以計算加權圖中任兩點間的最短距離。

具體實施過程：將根據圖2中各礦井節點即定坐標，根據兩點之間距離公式，計算得到頂點集中任意兩點間的距離矩陣，記為“d”；在Matlab中運行Dijkstra矩陣算法得到任意兩點間的最短路徑矩陣記為“dshortest”；找到簡化場地圖2中各點對應的最短距離，保存為矩陣“W”，這樣便得到了節點間最短距離矩陣即權值矩陣“W”。

可以看出W為對稱矩陣。對W的第k（k=1,2…,n）行運用Dijkstra算法，便可以求出頂點k到其他各個頂點的最短距離，找到的最短距離對應地保存在矩陣W的第k行，運算完畢得到的矩陣 W的元素值就是頂點集中任意兩個頂點之間的最短距離。

2 救援機器人路徑規劃代價規則的建立

2.1 權值矩陣賦值

根據圖5所示的模擬救援節點編號與得分對照圖，對權值矩陣W進行重新賦值。隨后在實際工作時，機器人想要在盡可能短的時間內查找發現盡可能多的救援節點，必然首先想要到達離自己當前所在位置最近而受傷情況最嚴重的節點，但是根據當前實際礦井下的實際情況，分值越高的節點，機器人到達所需的路程越長、難度系數越高，我們可以將二者比喻為到達一點的欲望與困難程度，這種“欲望”減弱了到達某一景點的“困難感”，而“困難程度”的增減也使得想要到達這一點的“欲望”變弱，顯然二者之間是反比關系，所以我們引入下列公式

2.2 添加角度函數

在上面的分析中，我們已經知道機器人在轉彎時，為了避免“脫離監控視線”的現象的發生，要進行相應的控制，因此，為了適當的避免轉彎動作造成的不必要的時間耗費我們在代價函數中添加，它是轉彎角度的遞增函數；我們規定：機器人右轉時，；機器人左轉時，；其中為0到1之間的小數，這使得機器人左轉的優先權高于右轉，這樣變換的好處是：在使得機器人在選擇路徑時盡可能的形成一個回路的同時通過引入此規則，機器人首先會考慮直行，其次是左轉，最后是右轉。規則中所設定的函數參數如、，需要在試驗中進行驗證，得出限定范圍內的較優值。

3 仿真與分析

根據救援場地環境和實際救援時的要求，在建立了機器人代價函數規則的基礎上，以兩點間最短路徑矩陣“W”為粒子群速度更新時的選路依據，引入離散粒子群算法求解TSP問題。在MATLAB中編程運行得到仿真結果：

圖6為每次迭代后，所找到的種群中粒子最優位置的值；圖7為最優值即適應度值隨迭代次數的進化曲線，它圖明了算法的性能，適應度值是否能在較短的時間內收斂于一個值。

圖6 每次迭代后粒子當前位置的最優值

圖7 適應度值隨迭代次數的進化曲線

圖6與圖7進行對比，可以看出算法迭代完成后找到的最優路徑為一條閉合沒有交叉的曲線，明顯優于算法初始化時找到的最優路徑，并且它達到了我們所期望的求解TSP問題的要求。圖6中剛開始迭代時，每次迭代后粒子當前位置的最優值有較大幅度的振蕩，但隨著迭代次數的增加，粒子當前位置的最優解成遞減函數并很快收斂于局部最優解；圖7中所得到的最優值平穩的過渡到下一個更小的全局最優解，這說明粒子群算法中社會學習行為能夠較好的提高算法的魯棒性和效率。實驗結果充分驗證了引入TSP理論來解決礦井救援機器人的路徑優化問題效果是良好的。

對應問題轉化過程中對景點的編號，我們得到機器人最優路徑1、3、2、4、7、6、5、8、9、10、11、12、13、14、15、17、16，最優路徑的權值為4478.1。如圖8所示。

圖8 機器人最優路徑

4 結論

本文在“礦井救援機器人”背景下，引入TSP的基本理論解決了機器人路徑規劃的難題。經過路徑規劃后的機器人游歷路徑，并沒有包含實際礦井環境中的所有礦井節點，而是通過代價函數的建立，自動舍棄了一些不太可能發生礦難得節點，這在一定程度上，降低了機器人在救援中出現浪費時間而導致救援延誤的可能性，其實際效果在機器人仿真和實際實驗中均取得了較好的效果。

[1] 葛平淑，王榮本，郭烈.基于模糊邏輯的六輪月球車路徑跟蹤控制[J].吉林大學學報(工學版)，2011；41(2)：503-508

[2] Laleh Haerian Ardekani Tiru S.Arthanari Matthias Ehrgott.Performance of the Branch and BoundAlgorithm on the Multistage InsertionFormulation of the Traveling SalesmanProblem[C].Proceedings of the 45th Annual Conference of the ORSNZ November 2010 pp. 326-335

[3] 王曉陵，陸軍．最優化方法和最優控制[M].哈爾濱：哈爾濱工程大學出版社，2006:256-300.

[4] 代西武.Dijkstra矩陣算法[J].北京建筑工程學院學報，2007；23(2)：65-67,71

[5] Qi Sun，Hui Liu，Qiang Yang，Wenjun Yan.On the Design for AGVs.Modeling[C]，Path Planning and Localization Proceedings of the 2011 IEEE International Conference on Mechatronics and Automation.Beijing,China.2011 pp.1515-1520

[6] S.LaValle，S.Hutchinson.Optimal motion planning for multiple robots having independent goals[C]. Proceedings of the 1996 IEEE/RSJ International Conference on Robotics and Automation. Osaka, Japan.1996：1619-1624

[7] S.Gao，B.Han and X.J.Wu.Solving traveling salesman problem by hybrid particle swarm optimization algorithm (In Chinese)[J].Control and Decision.2004，19(11)：1286-1289

[8] 朱小平，趙曦.一種改進的求解 TSP的粒子群算法[J].科學技術與工程，2010；10(15)：3727-3729

[9] 秦全德,李榮鈞.基于生物寄生行為的雙種群粒子群算法[J]. 控制與決策. 2011(04)：548-552

[10] 劉于江，喻澤峰.一種求解旅行商問題的禁忌搜索算法[J].江西理工大學學報, 2006；27(4)：38-40

[11] 唐文娟，劉傳領.改進的優化算法用于移動機器人路徑規劃[J].科學技術與工程，2012；12(29):7598-7602

Research on path planning of mine operation robot based on TSP theory

Cui Fujian
(Zhengzhou Coal Energy Development Co.,Ltd.Dengfeng Henan,452470)

Along with the national attention on automation,more and more of the mine began the introduction of mine robot for coal mining and mining,but mine under harsh environment and large mine complex internal structure,so downhole operation robot on the path planning ability have very high requirements.In this paper,we introduce the idea of traveling salesman problem for finding the optimal motion path;application of graph theory to mine structure schematic diagram analysis transformation,introduced Dijkstra algorithm to solve the shortest path problem and in the cost matrix add angle increasing function to determine the priority,at last,using the discrete particle swarm optimization algorithm is adopted to solve the model and achieved good experimental results.

mine working robot;path planning;traveling salesman problem;Dijkstra algorithm;discrete particle swarm optimization algorithm

崔伏建（1957-）男，漢族，河南省登封市徐莊鎮高坡村人，高級工程師，現任國投煤炭鄭州能源開發有限公司總經理，從事煤礦管理工作，本科，主要研究方向為煤礦井下人員定位技術研究、煤礦自動化等