機(jī)器人避障問題的最短路徑分析

1、機(jī)器人避障問題的最短路徑分析摘要本論文研究了機(jī)器人避障最短路徑和最短時間路徑的問題。主要討論了在一個區(qū)域中存在12個障礙物，由出發(fā)點(diǎn)到達(dá)目標(biāo)點(diǎn)以及由出發(fā)點(diǎn)經(jīng)過若干目標(biāo)點(diǎn)最終到達(dá)出發(fā)點(diǎn)的兩種情況。采用傳統(tǒng)的避障方法切線圖法。建立了線圓結(jié)構(gòu)，這樣任何路徑，我們都可以將路徑劃分為若干個這種線圓結(jié)構(gòu)來求解。對于途中經(jīng)過節(jié)點(diǎn)再到達(dá)目標(biāo)點(diǎn)的狀況，我們采用在轉(zhuǎn)彎點(diǎn)和節(jié)點(diǎn)都采用最小轉(zhuǎn)彎半徑，以節(jié)點(diǎn)為切點(diǎn)的形式。然后建立了最優(yōu)化模型，利用MATLAB軟件對方案進(jìn)行求解。問題一：把路徑分解成若干個線圓結(jié)構(gòu)來求解，然后把可能的最短路徑采用窮舉法列舉出來，最終得出最短路徑： 最短路徑為：471.0 最短路徑為：869

2、.5 最短路徑為：1093.3對于 我們將A、B、C看作切點(diǎn)，同樣采用線圓結(jié)構(gòu)計(jì)算。 最短路徑為：2827.1問題二：考慮避障路徑和轉(zhuǎn)彎速度，我們建立時間與路徑之間的模型，用MATLAB軟件求出最優(yōu)解。當(dāng)轉(zhuǎn)彎半徑為11.5的時候，可以得出最短時間為：T=94.3關(guān)鍵詞 最優(yōu)化模型 避障路徑 線圓結(jié)構(gòu) 切線圖法一、問題重述本文是求一個機(jī)器人在800×800的平面場景圖中避開障礙物，建立從原點(diǎn)O(0, 0)點(diǎn)處出發(fā)達(dá)到終點(diǎn)的最短路徑和最短時間路徑的模型。即求：1、OA、OB、OC和OABCO的最短路徑。2、OA的最短時間路徑。 機(jī)器人在行走時的要求是：1、它只能在該平面場景范圍內(nèi)活動2、

3、圖中有12個不同形狀的區(qū)域是機(jī)器人不能與之發(fā)生碰撞的障礙物（障礙物的分布如圖1）3、障礙物外指定一點(diǎn)為機(jī)器人要到達(dá)的目標(biāo)點(diǎn)（要求目標(biāo)點(diǎn)與障礙物的距離至少超過10個單位）。4、規(guī)定機(jī)器人的行走路徑由直線段和圓弧組成，其中圓弧是機(jī)器人轉(zhuǎn)彎路徑。機(jī)器人不能折線轉(zhuǎn)彎，轉(zhuǎn)彎路徑由與直線路徑相切的一段圓弧組成，也可以由兩個或多個相切的圓弧路徑組成，但每個圓弧的半徑最小為10個單位。5、為了不與障礙物發(fā)生碰撞，同時要求機(jī)器人行走線路與障礙物間的最近距離為10個單位，否則將發(fā)生碰撞。機(jī)器人直線行走的最大速度為個單位/秒。機(jī)器人轉(zhuǎn)彎時，最大轉(zhuǎn)彎速度為，其中是轉(zhuǎn)彎半徑。已知場景圖中4個點(diǎn)O(0, 0)，A(300

4、, 300)，B(100, 700)，C(700, 640)。圖中各個點(diǎn)的坐標(biāo)見下表。 圖1編號障礙物名稱左下頂點(diǎn)坐標(biāo)其它特性描述1正方形(300, 400)邊長2002圓形圓心坐標(biāo)(550, 450)，半徑703平行四邊形(360, 240)底邊長140，左上頂點(diǎn)坐標(biāo)(400, 330)4三角形(280, 100)上頂點(diǎn)坐標(biāo)(345, 210)，右下頂點(diǎn)坐標(biāo)(410, 100)5正方形(80, 60)邊長1506三角形(60, 300)上頂點(diǎn)坐標(biāo)(150, 435)，右下頂點(diǎn)坐標(biāo)(235, 300)7長方形(0, 470)長220，寬608平行四邊形(150, 600)底邊長90，左上頂點(diǎn)坐

5、標(biāo)(180, 680)9長方形(370, 680)長60，寬12010正方形(540, 600)邊長13011正方形(640, 520)邊長8012長方形(500, 140)長300，寬60 二、模型假設(shè)1、把機(jī)器人抽象成質(zhì)點(diǎn)。2、機(jī)器人直線行走都是以最大速度做勻速運(yùn)動。3、機(jī)器人轉(zhuǎn)彎時同樣以最大允許速度做勻速運(yùn)動。3、 符號說明 符號符號說明 每一個線圓結(jié)構(gòu)起點(diǎn)到終點(diǎn)的長度每一個線圓結(jié)構(gòu)起點(diǎn)到圓心的長度每一個線圓結(jié)構(gòu)終點(diǎn)到圓心的長度轉(zhuǎn)彎半徑（即題上的）轉(zhuǎn)彎圓心角轉(zhuǎn)彎圓心坐標(biāo)A到Z各個切點(diǎn)，節(jié)點(diǎn)的坐標(biāo) 每個弧長的長度第i段直線段的長度第j段圓弧的長度第i段直線段所用的時間第j段圓弧所用的時間d

6、障礙物上的任意點(diǎn)與行走路徑之間的最短距離四、模型分析與準(zhǔn)備一、最短路徑和最短時間路徑的分析 （1）問題一：要求從出發(fā)，沿,和的行走路線按照一定規(guī)則，繞過障礙物到達(dá)目標(biāo)點(diǎn)的最短路徑。在求，時。我們將所有的障礙物擴(kuò)大10個單位，在障礙物的頂點(diǎn)處，就是以10為半徑圓弧。我們采用拉繩子的方法尋找所有可能的最短路徑。然后利用線圓結(jié)構(gòu)的方式求解。即列舉法。（例如求解O到A的最短路徑，我們就可以連接和之間的一段繩子，以障礙5左上角的拐角處的圓弧為支撐拉緊，那么這段繩子的長度便是到的一條可能的最短路徑）。在求時，在過點(diǎn)、處我們采用最小半轉(zhuǎn)彎的方式，使機(jī)器人經(jīng)過這些過點(diǎn)時，都是按圓弧通過。其他情況就是按線圓結(jié)構(gòu)

7、的方式進(jìn)行求解。 （2）問題二、要求從O點(diǎn)出發(fā)到達(dá)A點(diǎn)的最短時間路徑問題，采用窮舉法和CAD畫圖可以得出最短時間路徑。二、在模型的建立中會大量用到線圓結(jié)構(gòu)來計(jì)算，證明起點(diǎn)到終點(diǎn)之間最短的路徑就是我們所用的線圓結(jié)構(gòu)。證明：如圖所示的線圓結(jié)構(gòu),和圓弧之和為最短避障路徑。 假設(shè)在平面中有和兩點(diǎn)，中間有一個半圓形的障礙物，證明從到的最路徑為圓弧和線段、的和。 圖2平面上連接兩點(diǎn)最短的路徑是通過這兩點(diǎn)的直線段，但是連接兩點(diǎn)的線段被障礙物遮擋，所以設(shè)法嘗試?yán)@過障礙物的折線路徑。在y軸上取一點(diǎn)，若y適當(dāng)大，當(dāng)折線與障礙物相切時，是這種折線路徑中最短的。由于滿足的角滿足,所以易知弧度小于的長，即證明到了線圓結(jié)

8、構(gòu)為最短避障路徑。線圓結(jié)構(gòu)的長度： 設(shè)， 為起始點(diǎn)，為目標(biāo)點(diǎn)，。求OA，設(shè)為L。 ： ： 因?yàn)樗裕?從而可得： 六、模型的建立與求解 假設(shè)機(jī)器人從起點(diǎn)R到到目標(biāo)點(diǎn)，由圖2知路徑一定是由圓弧和線段組成，設(shè)有m條線段，n條圓弧。那么最短路程：用此模型就可以對起點(diǎn)到目標(biāo)點(diǎn)之間的路徑進(jìn)行優(yōu)化求解。 時間模型： 問題一：1、由以上模型采用窮舉法知從的路徑線路通過簡化有如下兩種情況，分別為，如圖3：已知O(0，0)，D(80，210)，A(300，300) 圖3 用MATLAB軟件算得：OA的路徑為：471.0372所以：OA的最短路徑為圖3其結(jié)果為：471.0372 2、通過窮舉法分析知采用如下圖6所

9、示路線可以可以算出OB的長度。 設(shè)D到M點(diǎn)的坐標(biāo)分別為（）i= 1.10； Q1到Q5表示為繞過的各個障礙物的圓心設(shè)坐標(biāo)為（）j=11.15； 表示各直線相交的夾角； 類似的采用以上的算法可以得到如下直線的長度： 圖6 則：夾角為 所以O(shè)B所走路線的,弧線長度為： 每個線圓結(jié)構(gòu)起點(diǎn)到終點(diǎn)，起點(diǎn)到圓心，圓心到終點(diǎn)的距離： （j=1,2,3,4,5）故，OB的最短路徑的目標(biāo)函數(shù)為: 通過軟件計(jì)算知 :的最短路徑是869.4332 3、在通過分析知OC的最短路徑為圖5所示，通過visio畫圖得到如下圖所示：設(shè)其中j為1到4的整數(shù)，。A.B.J.I.D.K.E.F.G.H的坐標(biāo)分別為（）其中i為5到1

10、4的整數(shù)，M().各個點(diǎn)之間的關(guān)系：J點(diǎn)坐標(biāo) K點(diǎn)坐標(biāo) G點(diǎn)坐標(biāo) F點(diǎn)坐標(biāo)我們將O到C分成五個線圓結(jié)構(gòu)，根據(jù)上述模型分析可以得到：:所以:所以通過MATIAB軟件編程可以算出：OC的最短路徑為：1093.301 圖5 4、通過分析可知OABCO的最短路徑為如下圖7所示： 圖7因?yàn)镺ABCO的路線簡化成由16個線圓結(jié)構(gòu)組成的 : FL： LB： KN： MQ： Qt： SV： VY： XT： Tq：為了簡化模型，我們假設(shè)A、B、C點(diǎn)為切點(diǎn)。因?yàn)镺A和OC在前面已經(jīng)計(jì)算了最短值所以 綜上所述可得各個點(diǎn)的坐標(biāo)：OC的坐標(biāo)為： 圓弧圓心 (230，60) (410，100) (500,200) (72

11、0,520) （720,600） 切點(diǎn) （422，90） （428.7，94.5) (492，206） （727.6，514） （730,520） （730,600） (727.8，606.3)OB的坐標(biāo)為： 圓心 （60,300） (150,435) (220,470) （220,530） （150,600） 切點(diǎn)（50.13，301.06） （51.7，305.5） （141.7，439.6） （230,530) （150,444.03） （222.1，460.2） （140.7,596.3） （230,470） (225.5，538.3) （144.5，591.6） OA的坐標(biāo)為：圓心（

12、80,210） 切點(diǎn)（70.5，213） （76.6，219.4）的坐標(biāo)圓弧切點(diǎn)（70.5,213.1） （76.6，219） （291.07,297.78） （297.25,309.08）（229.25,532.9） （225.5,538.35） （144.5,591.64） （140.7,596.35）（105.7,685.4） （115.6,699） （270.6,689.9） （272,689.8）（366.7 ，670.3） (533.9 ，738.3) （699.4 ，642.3） （369.3 ，670） （539.6 ，740） （429.3， 670） （539.5 ，740

13、） （435.1 ，671.8） （679.3 ，732.18）圓心（80， 210） （287.68 ，306.18） （220 ，530） （150,600）（115.02，639） （270,680） （369.3 ，680） （429.3 ，680） （539.5 ，730） （669.5 ，730） （709.2 ，644.5） 問題二：通過分析采用圖9的路線可以得出最短時間路徑如圖所示： 圖9是O到A的最短時間路徑 其中C點(diǎn)，B點(diǎn)分別是直線與圓弧的兩個切點(diǎn)：CB長度是d。 直線的斜率為，直線的斜率為。所以直線的方程：·····

14、83;·························(1)直線的方程：·······················

15、·······(2)將式子(1)和式子(2)連理求解可以得到圓心坐標(biāo)。圓弧所對的圓心角為 由到角公式可知：···································&#

16、183;(3)因?yàn)?所以有兩直線垂直那么其斜率相乘等于零。因此：············································

17、3;·(4) ················································

18、··(5)在中顯然有····································(6)將式子(3)、(4)、(5)、(6)連理求解可以得到和由于轉(zhuǎn)彎的的半徑還受到障礙物5和障礙物6的影響，所以切點(diǎn)的半徑有限制

19、范圍，通過計(jì)算可限制切點(diǎn)的半徑范圍： 又因?yàn)镈點(diǎn)必須在兩條切線的內(nèi)側(cè)，所以我們通過 的斜率大于;的斜率小于來限制。即： 所以最短時間可以表示為：以上式子用MATLAB求解得到最短時間為：95.4332秒在上面的模型中是圓心固定算出半徑是11.5，如果不固定圓心O1，那么圓心的范圍就是一個以O(shè)點(diǎn)為圓心，1.5為半徑的小圓內(nèi)部的點(diǎn)。為了方便起見將圓心O1坐標(biāo)的取值定在小圓的內(nèi)切正方形里面。然后利用上面的模型加上（7）式的約束條件可以在這個小正方形里面找到最優(yōu)解。 ···········&#

20、183;······································（7）上面式子的程序再加上（7）這個約束條件用MATLAB求解得到最短時間為：94.3秒.兩個切點(diǎn)坐標(biāo) 七、模型評價(jià)（一）優(yōu)點(diǎn)：1.文章

21、中的計(jì)算過程皆運(yùn)用數(shù)學(xué)軟件求解，且求解過程簡潔，使得出的數(shù)據(jù)更具有說服力；2.本文通過對問題的充分分析，在合理的假設(shè)情況下，建立了具有科學(xué)性的方程模型；3.運(yùn)用本文模型求解相關(guān)問題時結(jié)果與實(shí)際情況基本相吻合；（二）缺點(diǎn)：問題（一）中其他幾問的算法基本上同OA的算法一致，數(shù)據(jù)較大，在處理時存在一定的誤差，但誤差在允許的范圍內(nèi)，可以接受。八、模型檢驗(yàn)與推廣 本文是利用傳統(tǒng)方法切線圖法求解切線圖法用障礙物的切線表示弧，此時移動機(jī)器人必須接近障礙物，在有誤差的時候可能與障礙物有接觸，但解決了障礙物不能是圓形的問題。我們可以采用基于遺傳算法與人工神經(jīng)網(wǎng)絡(luò)的智能避障算法。九、參考文獻(xiàn)1 2 陽明盛，熊西文

22、，林建華，MATLAB基礎(chǔ)及數(shù)學(xué)軟件，大連：大連理工大學(xué)出版社，2003年。3移動機(jī)器人避障方法綜述 作者 牢常健， 吳成東， 李斌 科學(xué)院沈陽動化研究所機(jī)器人學(xué)田家重點(diǎn)實(shí)驗(yàn)室；中嗣科學(xué)院研究生院；東 北大學(xué)信息科學(xué)與工程學(xué)院十，附錄附錄一；為最短路線OA 的程序，是利用MATIAB求解的%0：初始點(diǎn) D:轉(zhuǎn)彎圓弧圓心 A:到達(dá)點(diǎn) r:表示圓弧半徑function result=zongchang1(r)O(1)=0;O(2)=0;A(1)=300;A(2)=300;D(1)=80;D(2)=210;OD = sqrt(O(1)-D(1)2+(O(2)-D(2)2);OA=sqrt(O(1)-

23、A(1)2+(O(2)-A(2)2);AD=sqrt(D(1)-A(1)2+(D(2)-A(2)2);alpha1=acos(OD2+AD2-OA2)/(2*OD*AD);alpha2 = acos(r/OD);alpha3 = acos(r/AD);alpha4=2*pi-alpha1-alpha2-alpha3;%alpha4為轉(zhuǎn)彎圓心角OS1=sqrt(OD2-r2);%OS1,AS2均為圓弧切線%S2A=sqrt(AD2-r2);S1S2hu=r*alpha4;result=OS1+S1S2hu+S2A;endzongchang1(10)ans = 471.0372附錄二；為最短路線O

24、B的程序，是利用MATIAB編程的O-F%求解一次轉(zhuǎn)彎所經(jīng)路線總長%0：初始點(diǎn) Q1:轉(zhuǎn)彎圓弧圓心 F:到達(dá)點(diǎn) r:表示圓弧半徑function result=zongchangob1(r)O(1)=0;O(2)=0;F(1)=141.675;F(2)=440.55;Q1(1)=60;Q1(2)=300;OQ1 = sqrt(O(1)-Q1(1)2+(O(2)-Q1(2)2);OF=sqrt(O(1)-F(1)2+(O(2)-F(2)2);FQ1=sqrt(Q1(1)-F(1)2+(Q1(2)-F(2)2);alpha1=acos(OQ12+FQ12-OF2)/(2*OQ1*FQ1);alph

25、a2 = acos(r/OQ1);alpha3 = acos(r/FQ1);alpha4=2*pi-alpha1-alpha2-alpha3;%alpha4為轉(zhuǎn)彎圓心角OD=sqrt(OQ12-r2);%OD,FE均為圓弧切線%EF=sqrt(FQ12-r2);DEhu=r*alpha4;result=OD+DEhu+EF;end zongchangob1(10)F-P%求解一次轉(zhuǎn)彎所經(jīng)路線總長% E：初始點(diǎn) Q2:轉(zhuǎn)彎圓弧圓心 P:到達(dá)點(diǎn) r:表示圓弧半徑function result=zongchangob1(r)E(1)=51.675;E(2)=305.5;P(1)=185;P(2)=4

26、52.5;Q2(1)=150;Q2(2)=435;EQ2 = sqrt(E(1)-Q2(1)2+(E(2)-Q2(2)2);EP=sqrt(E(1)-P(1)2+(E(2)-P(2)2);PQ2=sqrt(Q2(1)-P(1)2+(Q2(2)-P(2)2);alpha1=acos(EQ22+PQ22-EP2)/(2*EQ2*PQ2);alpha2 = acos(r/EQ2);alpha3 = acos(r/PQ2);alpha4=2*pi-alpha1-alpha2-alpha3;%alpha4為轉(zhuǎn)彎圓心角EF=sqrt(EQ22-r2);%EF,PG均為圓弧切線%PG=sqrt(PQ22-r

27、2);FGhu=r*alpha4;result=FGhu+PG;endP-J%求解一次轉(zhuǎn)彎所經(jīng)路線總長% P：初始點(diǎn) Q3:轉(zhuǎn)彎圓弧圓心 J:到達(dá)點(diǎn) r:表示圓弧半徑function result=zongchangob1(r)J(1)=230;J(2)=530;P(1)=185;P(2)=452.5;Q3(1)=220;Q3(2)=470;PQ3 = sqrt(P(1)-Q3(1)2+(P(2)-Q3(2)2);JP=sqrt(J(1)-P(1)2+(J(2)-P(2)2);JQ3=sqrt(Q3(1)-J(1)2+(Q3(2)-J(2)2);alpha1=acos(JQ32+PQ32-JP

28、2)/(2*JQ3*PQ3);alpha2 = acos(r/PQ3);alpha3 = acos(r/JQ3);alpha4=2*pi-alpha1-alpha2-alpha3;%alpha4為轉(zhuǎn)彎圓心角JI=sqrt(JQ32-r2);%JI,PH均為圓弧切線%PH=sqrt(PQ32-r2);HIhu=r*alpha4;result=PH+HIhu+JI;endJ-q%求解一次轉(zhuǎn)彎所經(jīng)路線總長% P：初始點(diǎn) Q3:轉(zhuǎn)彎圓弧圓心 J:到達(dá)點(diǎn) r:表示圓弧半徑function result=zongchangob1(r)J(1)=230;J(2)=530;P(1)=185;P(2)=452.

29、5;Q3(1)=220;Q3(2)=470;PQ3 = sqrt(P(1)-Q3(1)2+(P(2)-Q3(2)2);JP=sqrt(J(1)-P(1)2+(J(2)-P(2)2);JQ3=sqrt(Q3(1)-J(1)2+(Q3(2)-J(2)2);alpha1=acos(JQ32+PQ32-JP2)/(2*JQ3*PQ3);alpha2 = acos(r/PQ3);alpha3 = acos(r/JQ3);alpha4=2*pi-alpha1-alpha2-alpha3;%alpha4為轉(zhuǎn)彎圓心角JI=sqrt(JQ32-r2);%JI,PH均為圓弧切線%PH=sqrt(PQ32-r2);

30、HIhu=r*alpha4;result=PH+HIhu+JI;endq-B%求解一次轉(zhuǎn)彎所經(jīng)路線總長% q：初始點(diǎn) Q5:轉(zhuǎn)彎圓弧圓心 B:到達(dá)點(diǎn) r:表示圓弧半徑function result=zongchangob1(r)B(1)=100;B(2)=700;q(1)=185;q(2)=565;Q5(1)=150;Q5(2)=600;BQ5= sqrt(B(1)-Q5(1)2+(B(2)-Q5(2)2);Bq=sqrt(B(1)-q(1)2+(B(2)-q(2)2);qQ5=sqrt(Q5(1)-q(1)2+(Q5(2)-q(2)2);alpha1=acos(BQ52+qQ52-Bq2)/

31、(2*BQ5*qQ5);alpha2 = acos(r/qQ5);alpha3 = acos(r/BQ5);alpha4=2*pi-alpha1-alpha2-alpha3;%alpha4為轉(zhuǎn)彎圓心角BM=sqrt(BQ52-r2);%BM,qL均為圓弧切線%qL=sqrt(qQ52-r2);LMhu=r*alpha4;result=qL+LMhu+BM;endfunction oc (a,b,c,d,e);sum5 = zongchangob0F(10)+zongchangobFP(10)+zongchangobPJ(10)+zongchangobJq(10)+zongchangobqB(1

32、0)869.4896附錄三；為最短時間路線OA的程序（圓心固定時），是利用MATIAB編程的f>%是假設(shè)圓心固定所得出的模型%E：初始點(diǎn) F:轉(zhuǎn)彎圓弧圓心 G:到達(dá)點(diǎn) for i=1:7000; r(i)=10+0.01*i;E(1)=0;E(2)=0;F(1)=80;F(2)=210;G(1)=300;G(2)=300;EF=sqrt(E(1)-F(1)2+(E(2)-F(2)2);%bEG=sqrt(E(1)-G(1)2+(E(2)-G(2)2);%aFG=sqrt(F(1)-G(1)2+(F(2)-G(2)2);%calpha1=acos(EF2+FG2-EG2)/(2*EF*FG

33、);% alpha1為起始點(diǎn)與圓心連線的夾角alpha2=acos(r(i)/EF);% alpha2為起點(diǎn)到圓心與切點(diǎn)連線的夾角alpha3=acos(r(i)/FG);% alpha3為起點(diǎn)到圓心與切點(diǎn)連線的夾角alpha4=2*pi-alpha1-alpha2-alpha3;%alpha4為轉(zhuǎn)彎圓心角ES1=sqrt(EF2-r(i)2);%ES1,ES2均為圓弧切線%S2G=sqrt(FG2-r(i)2);S1S2hu=r(i)*alpha4;L(i)=ES1+S1S2hu+S2G;%總路程a(i)=ES1/5+S2G/5+S1S2hu*(1+exp(10-0.1*r(i)2)/5;%總時間endmintime=min(a) %最小時間k=find(a=mintime)r(k)%最小時間時候的半徑L(