基于圖的理論解決機器人避障最短路徑選擇問題

張 琪，王一帆

（承德醫(yī)學院，河北承德 067000）

隨著智能機器人的廣泛應(yīng)用，人們對機器人避障問題從各個方面、以多種方法進行研究探討。本文就機器人避障最短路徑選擇問題，用數(shù)學方法采集數(shù)據(jù)并進行計算，以有向圖的理論篩選數(shù)據(jù)并建立數(shù)學模型，再用C語言按Dijkstra算法編寫代碼，從而實現(xiàn)求最短行走和最短時間路徑。

1 問題提出

假設(shè)一個800×800的場景圖（見圖1）。機器人只能在該場景內(nèi)活動。場景內(nèi)有12個各種形狀的障礙物，機器人不能與它們發(fā)生碰撞，障礙物的描述見附表。機器人從指定一點到達另一點，行走路徑由直線段和圓弧組成。轉(zhuǎn)彎的路徑是由一段圓弧組成(與直線路徑相切)，也可以由多于一個相切的圓弧組成，圓弧的半徑最小為10個單位。要求行走的線路距離障礙物最少為10個單位，機器人行走時，直線最大速度為v0=5個單位/秒，轉(zhuǎn)彎最大速度為，其中ρ是轉(zhuǎn)彎行走時的半徑。

要求建立機器人在場景從某點到達其它點行走路線的最短路徑和最短時間路徑的數(shù)學模型，并對場景圖中的4個點O(0， 0)、A(300， 300)、B(100， 700)、C(700，640)進行計算：⑴機器人從O(0，0)出發(fā)，O→A、O→B、O→C和O→A→B→C→O的最短行走路徑；⑵機器人從O(0，0)出發(fā)，O→A的最短時間路徑。

圖1 場景圖

附表 障礙物描述

2 解決方法

2.1 采集數(shù)據(jù) 找出機器人從任意點出發(fā)，經(jīng)最短路徑到達其它各點可能經(jīng)過點，給出點的坐標、弧的圓心，并求出線段長和弧長。在這里用到了初等數(shù)學中的兩點間距離公式、點到直線距離公式、圓的內(nèi)公切線方程和外公切線方程、求弧長公式、圓方程等。計算方法：以11個非圓型障礙物的各頂點為圓心，10為半徑構(gòu)造圓的方程。以2號障礙物圓心為圓心，80為半徑構(gòu)造圓方程。共34個圓，可能的最短路徑經(jīng)過22個圓。各直線段的計算方法是，從點O、A、B、C出發(fā)的直線段，先求過圓外一點到已知圓的切線，再求切點；其它位置的各線段，用與兩已知圓的內(nèi)公切線方程或外公切線方程，再求這條公切線與兩已知圓切點；最后，以兩點間距離公式求線段長度。弧長的求法：根據(jù)上面求直線段的方法，每條弧都與兩條直線段相切，通過兩個切點連接的一條弦，求出半徑為r(r=10或r=80)所對應(yīng)的弧長。這里我們經(jīng)過計算，得到了滿足條件的所有線段和弧長，這些線段和圓弧構(gòu)成了圖1中的有向網(wǎng)。愈求得最短路徑和最短時間路徑，機器人可能經(jīng)過的點得到如下數(shù)據(jù)(為了計算方便，交點坐標取整數(shù))：

（1）O到B所有可能經(jīng)過的線段：

線段O(0，0)到D(70，211)，長度222.31，弧D的圓心(80，210)，弧長=9.2。

線段D(79，219)到F(235，290)，長度171.4，弧F的圓心(245，290)，弧長=15.7。

線段F (245，300)到K(230，530)，長度230.48，弧K的圓心(220，530)，弧長=13.9。

線段K(221，539)到L(151，591)，長度87.2；弧L的圓心(150，600)，弧長=12.8。

線段L到B(100，700)，長度107.71。

線段F(245，300)到G(280，680)，長度380，弧G的圓心(270，680)，弧長=15.7。

線段G(270，690)到B(100，700)，長度170.3。

線段O(0，0)到E(231，50)，長度為236.35，弧E的圓心(230，60)，弧長=13.76。

線段E(240，59)到G(280，680)，長度為622.29。

線段O(0，0)到H(40，300)，長度302.65，弧H的圓心(50，300)，弧長=12。

線段H(48，308)到I(140，435)，長度156.82，弧I的圓心(150，435)，弧長=15.7。

線段I(150，445)到J(220，460)，長度71.58；弧J的圓心(220，470)，弧長=15.7。

線段J(230，470)到K(230，530)，長度60。

（2）O到C所有可能經(jīng)過的線段：

線段O(0，0)到M(412，90)，長度421.7，弧M的圓心(410，100)，弧長=11.64。

線段M(419，99)到N(489，201)，長度123.71，弧N的圓心(500，200)，弧長=12.92。

線段N(498，209)到R(721，511)，長度375.4，弧R圓心(720，520)，弧長=13.8。

線段R(730，520)到S(730，600)，長度80，弧S的圓心(720，600)，弧長=15.7。

線段S(719，610)到C(700，640)，長度31.32。

線段E(240，60)到P(391，331)，長度308.87，弧P的圓心(400，330)，弧長=12.08。

線段P(399，339)到Q(590，370)，長度193.5，弧Q的圓心(590，380)，弧長=13.8。

線段Q(599，379)到R(720，510)，長度178.33。

線段D(79，219)到P(391，331)，長度331.49。

（3）O到A所有可能經(jīng)過的線段：

線段O(0，0)到E(231，50)，長度236.35，弧E的圓心(230，60)，弧長=13.76。

線段E(240，59)到A(300，300)，長度248.36。

線段O(0，0)到D(70，211)，長度222.31，弧D的圓心(80，210)，弧長=10.2。

線段D(79，220)到A(300，300)，長度235（其中O到v3、O到v1與上面重復(fù)）。

（4）A到B所有可能經(jīng)過的線段：

線段A(300，300)到K(230，530)，長度264.2.(K到B以上線段已計算)。

線段A(300，300)到T(300，390)，長度90，弧T的圓心(300，400)，弧長=15.7。

線段T(290，400)到G(280，680)，長度280.18 (G到B以上線段已計算)。

（5）B到C所有可能經(jīng)過的線段：

線段A(300，300)到T(300，390)，長度90，弧T的圓心(300，400)，弧長=15.7。

線段T(290，400)到G(280，680)，長度280.18 (G到B以上線段已計算)。

（6）B到C所有可能經(jīng)過的線段：

線段B到G(280，680)的所有可能線段已經(jīng)計算，總長為170.3+15.7，弧G的圓心(280，690)，弧長=13。

線段G(270，695)到U(360，680)，長度91.24，弧U的圓心(430，680)，弧長=13.76。

線段U(439，679)到V(531，729)，長度104.71，弧V的圓心(540，730)，弧長=9.34。

線段V(540，740)到W(670，740)，長度130，弧W的圓心(670，730)，弧長=15.7。

線段W(680，730)到C(733，400)，長度92.2。

線段G(280，680)到X(501，609)，長度232.12，弧X的圓心(500，600)，弧長=11.88。

線段X(509，601)到Y(jié)(631，519)，長度147，弧Y的圓心(640，520)，弧長=13.72。

線段Y(640，510)到R(720，510)，長度80(R到C上面已計算)。

2.2 篩選數(shù)據(jù)—建立數(shù)學模型 根據(jù)問題找出滿足條件的所有線段和弧，構(gòu)成一有向圖，將入度和出度均為1的連續(xù)線段的長度相加，歸并為一條弧（圖論中將有向段稱為弧）。可以由以上有向圖中的各點找出其入度或出度不是1的點，構(gòu)成以下（拓撲）有向圖（網(wǎng)）[1]，即圖2（根據(jù)問題，如需要，弧均為雙向，圖中粗線是雙向）。

圖2 有向圖

根據(jù)圖2得到如下數(shù)據(jù)：

線段點O到點K，長度568.11。

線段點O到點D，長度232.51。

線段點O到點E，長度250.11。

線段點O到點R，長度1085.9。

線段點D到點F，長度187.1。

線段點D到點P，長度331.49。

線段點D到點A，長度235。

線段點E到點A，長度248.36。

線段點E到點G(260，700)，長度620.2。

線段點E到點P，長度308.87。

線段點F到點K，長度230.48。

線段點F到點G(260，700)，長度377。

線段點G(280，680)到點B，長度185.12。

線段點G(260，700)到點R，長度498.72。

線段點G(270，690)到點C，長度521.41。

線段點B到點G(280，680)，長度185.29。

線段點G(260，700)到點C，長度525.96。

線段點R到點C，長度140.82。

線段點K到點B，長度221.61。

線段點P到點R長度383.91。

線段點A到點K長度264.2。

2.3 選擇最短行走路徑 根據(jù)圖論的Dijkstra算法[2]，采用C語言編程[3]（鄰接矩陣存儲）。由于完整程序已經(jīng)正確運行，由于篇幅有限，僅給出求最小路徑函數(shù)代碼。主要代碼：

作為本程序的一個例子，運行程序，將所得數(shù)據(jù)按要求輸入，給出各點間的最短路徑。程序運行結(jié)果：

O-＞A 最短行走路徑為：O-＞D-＞A，長度467.51。

O-＞C 最短行走路徑為：O-＞R-＞C，長度1085.9。

O-＞B 最短行走路徑為：O-＞K-＞B，長度789.71(O-＞K 568.11，K-＞B 221.61)。

A-＞B 最短行走路徑為：A-＞K-＞B，長度485.81(A-＞K 264.2，K-＞B 221.61)。

B-＞C 最短行走路徑為：B-＞G-＞C，長度691.7(B-＞G 170，G-＞C 521.41)。

2.4 計算最短時間路徑 根據(jù)公式：距離=時間×速度（S=T×V）。在直線段上，T=S/V0 (V0=5)， 每段線段長除以V0即為最短時間。在弧上，按速度公式，求得V(ρ)=V0/2 (ρ=10)，每段弧長除以V(ρ)即為最短時間。

數(shù)學模型仍是圖2，以O(shè)到A最短時間路徑為例，求得數(shù)據(jù)為：

線段O(0，0)到E(231，50)，時間47.27；弧E的圓心(230，60)，時間=5.504。

線段E(240，59)到A(300，300)，時間49.672。

O-＞E-＞A，時間102.446。

線段O(0，0)到D(70，210)，時間44.46；弧D的圓心(80，210)，時間=4.008。

線段D(79，220)到A(300，300)，時間47。其中O到E(231，50)，O到D(70，210)同上面。

O-＞D-＞A，時間95.468。

得O-＞A的最短時間路徑為：O-＞D-＞A，最短時間為95.468。

若想求得所有最短時間路徑，如上方法計算，運行程序便得到結(jié)果。

3 結(jié)論

在解決機器人避障這個問題時，人們常用擬合的方法建立數(shù)學模型，利用窮舉法或者啟發(fā)式算法求解。這里用了精確的數(shù)學方法建立數(shù)學模型，并且應(yīng)用圖的最短路徑方法，以C語言編程，將這個問題作為一個例子來處理，給出了解決整個問題的完整而又精確的數(shù)學方法，希望對相關(guān)業(yè)者有所幫助。

【參考文獻】

[1]張琪，高紅亞.與障礙問題有關(guān)的一些正則性結(jié)果[J].應(yīng)用數(shù)學，2017，30(3)：644-651.

[2]嚴蔚敏.數(shù)據(jù)結(jié)構(gòu)[M].北京：清華大學出版社，2007.187-189.

[3]譚浩強.C語言程序設(shè)計[M].北京：清華大學出版社，2008.110-112.