基于圖的理論解決機(jī)器人避障最短路徑選擇問(wèn)題

張 琪，王一帆

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

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

1 問(wèn)題提出

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

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

圖1 場(chǎng)景圖

附表 障礙物描述

2 解決方法

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

圖2 有向圖

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

線段點(diǎn)O到點(diǎn)K，長(zhǎng)度568.11。

線段點(diǎn)O到點(diǎn)D，長(zhǎng)度232.51。

線段點(diǎn)O到點(diǎn)E，長(zhǎng)度250.11。

線段點(diǎn)O到點(diǎn)R，長(zhǎng)度1085.9。

線段點(diǎn)D到點(diǎn)F，長(zhǎng)度187.1。

線段點(diǎn)D到點(diǎn)P，長(zhǎng)度331.49。

線段點(diǎn)D到點(diǎn)A，長(zhǎng)度235。

線段點(diǎn)E到點(diǎn)A，長(zhǎng)度248.36。

線段點(diǎn)E到點(diǎn)G(260，700)，長(zhǎng)度620.2。

線段點(diǎn)E到點(diǎn)P，長(zhǎng)度308.87。

線段點(diǎn)F到點(diǎn)K，長(zhǎng)度230.48。

線段點(diǎn)F到點(diǎn)G(260，700)，長(zhǎng)度377。

線段點(diǎn)G(280，680)到點(diǎn)B，長(zhǎng)度185.12。

線段點(diǎn)G(260，700)到點(diǎn)R，長(zhǎng)度498.72。

線段點(diǎn)G(270，690)到點(diǎn)C，長(zhǎng)度521.41。

線段點(diǎn)B到點(diǎn)G(280，680)，長(zhǎng)度185.29。

線段點(diǎn)G(260，700)到點(diǎn)C，長(zhǎng)度525.96。

線段點(diǎn)R到點(diǎn)C，長(zhǎng)度140.82。

線段點(diǎn)K到點(diǎn)B，長(zhǎng)度221.61。

線段點(diǎn)P到點(diǎn)R長(zhǎng)度383.91。

線段點(diǎn)A到點(diǎn)K長(zhǎng)度264.2。

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

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

O-＞A 最短行走路徑為：O-＞D-＞A，長(zhǎng)度467.51。

O-＞C 最短行走路徑為：O-＞R-＞C，長(zhǎng)度1085.9。

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

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

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

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

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

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

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

O-＞E-＞A，時(shí)間102.446。

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

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

O-＞D-＞A，時(shí)間95.468。

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

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

3 結(jié)論

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

【參考文獻(xiàn)】

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

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

[3]譚浩強(qiáng).C語(yǔ)言程序設(shè)計(jì)[M].北京：清華大學(xué)出版社，2008.110-112.