崔伏建

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

基于TSP理論的礦井作業(yè)機器人路徑規(guī)劃問題探究

崔伏建

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

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

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

0 引言

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

1 “復(fù)雜環(huán)境下礦井救援機器人路徑規(guī)劃”向“TSP問題”的轉(zhuǎn)化

1.1 礦井節(jié)點坐標的建立

圖1 礦井結(jié)點編號對照

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

1.2 簡化場地圖

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

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

圖2 簡化礦井分布圖1

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

圖3 簡化礦井分布圖2

本文所設(shè)計的相應(yīng)的權(quán)值計算公式為W(23，33)=W(23，25)+W(25，33)，并以此類推。

（3）最終通過權(quán)值轉(zhuǎn)換得到的轉(zhuǎn)化圖如圖4所示：

圖4 簡化礦井分布圖3

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

圖5 結(jié)點編號轉(zhuǎn)換

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

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

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

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

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

2.1 權(quán)值矩陣賦值

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

2.2 添加角度函數(shù)

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

3 仿真與分析

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

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

圖6 每次迭代后粒子當前位置的最優(yōu)值

圖7 適應(yīng)度值隨迭代次數(shù)的進化曲線

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

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

圖8 機器人最優(yōu)路徑

4 結(jié)論

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

[1] 葛平淑，王榮本，郭烈.基于模糊邏輯的六輪月球車路徑跟蹤控制[J].吉林大學(xué)學(xué)報(工學(xué)版)，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] 王曉陵，陸軍．最優(yōu)化方法和最優(yōu)控制[M].哈爾濱：哈爾濱工程大學(xué)出版社，2006:256-300.

[4] 代西武.Dijkstra矩陣算法[J].北京建筑工程學(xué)院學(xué)報，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].科學(xué)技術(shù)與工程，2010；10(15)：3727-3729

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

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

[11] 唐文娟，劉傳領(lǐng).改進的優(yōu)化算法用于移動機器人路徑規(guī)劃[J].科學(xué)技術(shù)與工程，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-）男，漢族，河南省登封市徐莊鎮(zhèn)高坡村人，高級工程師，現(xiàn)任國投煤炭鄭州能源開發(fā)有限公司總經(jīng)理，從事煤礦管理工作，本科，主要研究方向為煤礦井下人員定位技術(shù)研究、煤礦自動化等