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

1、機(jī)器人避障問題的最短路徑分析摘要本論文研究了機(jī)器人避障最短路徑和最短時(shí)間路徑的問題。主要討論了在一個(gè)區(qū)域中存在12個(gè)障礙物，由出發(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)，這樣任何路徑，我們都可以將路徑劃分為若干個(gè)這種線圓結(jié)構(gòu)來求解。對(duì)于途中經(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件對(duì)方案進(jìn)行求解。問題一：把路徑分解成若干個(gè)線圓結(jié)構(gòu)來求解，然后把可能的最短路徑采用窮舉法列舉出來，最終得出最短路徑：Ot A最短路徑為：471.0O

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

3、C和OAf Bf CO 的最短路徑。2、OA的最短時(shí)間路徑。機(jī)器人在行走時(shí)的要求是：1、它只能在該平面場(chǎng)景范圍內(nèi)活動(dòng) 2、圖中有 12個(gè)不同形狀的區(qū)域是機(jī)器人不能與之發(fā)生碰撞的障礙物(障礙物的分布如圖 1) 3、障礙物外指定一點(diǎn)為機(jī)器人要到達(dá)的目標(biāo)點(diǎn)(要求目標(biāo)點(diǎn)與障礙物的距離至少超過10個(gè)單位)。4、規(guī)定機(jī)器人的行走路徑由直線段和圓弧組成，其中圓 弧是機(jī)器人轉(zhuǎn)彎路徑。機(jī)器人不能折線轉(zhuǎn)彎，轉(zhuǎn)彎路徑由與直線路徑相切的一段 圓弧組成，也可以由兩個(gè)或多個(gè)相切的圓弧路徑組成， 但每個(gè)圓弧的半徑最小為 10個(gè)單位。5、為了不與障礙物發(fā)生碰撞，同時(shí)要求機(jī)器人行走線路與障礙物間 的最近距離為10個(gè)單位，否則將

4、發(fā)生碰撞。機(jī)器人直線行走的最大速度為v°=5個(gè)單位/秒。機(jī)器人轉(zhuǎn)彎時(shí)，最大轉(zhuǎn)彎速 度為v =v(門，其中'是轉(zhuǎn)彎半徑。1 +e*已知場(chǎng)景圖中 4個(gè)點(diǎn)0(0, 0)，A(300, 300)，B(100, 700)，C(700, 640)。圖中各個(gè)點(diǎn) 的坐標(biāo)見下表。800600-100200200600圖1編號(hào)1障礙物名稱左下頂點(diǎn)坐標(biāo)其它特性描述廠正方形(300, 400)邊長2002圓形圓心坐標(biāo)(550, 450)，半徑703 n平行四邊形(360, 240)底邊長140,左上頂點(diǎn)坐標(biāo)(400, 330)8004三角形(280, 100)上頂點(diǎn)坐標(biāo)(345, 210)，右下頂點(diǎn)

5、坐標(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)坐標(biāo)(180, 680)9長方形(370, 680)長60,寬12010正方形(540, 600)邊長13011 1正方形(640, 520)邊長8012長方形(500, 140)長300,寬60模型假設(shè)1、把機(jī)器人抽象成質(zhì)點(diǎn)。2、機(jī)器人直線行走都是以最大速度做勻速運(yùn)動(dòng)。3、機(jī)器人轉(zhuǎn)彎時(shí)同樣以最大允許速度做勻速運(yùn)動(dòng)三、符號(hào)說明符號(hào)符號(hào)說明ai每一個(gè)線圓結(jié)構(gòu)起

6、點(diǎn)到終點(diǎn)的長度b每一個(gè)線圓結(jié)構(gòu)起點(diǎn)到圓心的長度q每一個(gè)線圓結(jié)構(gòu)終點(diǎn)到圓心的長度r轉(zhuǎn)彎半徑(即題上的P)q轉(zhuǎn)彎圓心角Oi(Xi,yJ轉(zhuǎn)彎圓心坐標(biāo)A到 Z(Xj,yJ各個(gè)切點(diǎn)，節(jié)點(diǎn)的坐標(biāo)li每個(gè)弧長的長度Li第i段直線段的長度Kj第j段圓弧的長度ti第i段直線段所用的時(shí)間Pj第j段圓弧所用的時(shí)間d障礙物上的任意點(diǎn)與行走路徑之間的最短距離四、模型分析與準(zhǔn)備最短路徑和最短時(shí)間路徑的分析(1 )問題一：要求從0(0,0)出發(fā)，沿OA,OB,OC 和Or Ar Br Cr 0的行走路線按照一定規(guī)則，繞過障礙物到達(dá)目標(biāo)點(diǎn)的最短 路徑。在求0A, 0 > B，0 > C時(shí)。我們將所有的障礙物擴(kuò)大1

7、0個(gè)單位， 在障礙物的頂點(diǎn)處，就是以10為半徑圓弧。我們采用拉繩子的方法尋找所有可 能的最短路徑。然后利用線圓結(jié)構(gòu)的方式求解。即列舉法。(例如求解0到A的最短路徑，我們就可以連接0和A之間的一段繩子，以障礙5左上角的拐角處的 圓弧為支撐拉緊，那么這段繩子的長度便是 0到A的一條可能的最短路徑)。在 求0r Ar B C 0時(shí)，在過點(diǎn)A、B、C處我們米用最小半轉(zhuǎn)彎的方式， 使機(jī)器人經(jīng)過這些過點(diǎn)時(shí)，都是按圓弧通過。其他情況就是按線圓結(jié)構(gòu)的方式進(jìn) 行求解。(2)問題二、要求從0點(diǎn)出發(fā)到達(dá)A點(diǎn)的最短時(shí)間路徑問題，采用窮舉法 和CADB圖可以得出最短時(shí)間路徑。二、在模型的建立中會(huì)大量用到線圓結(jié)構(gòu)來計(jì)算，

8、 證明起點(diǎn)到終點(diǎn)之間最短的路 徑就是我們所用的線圓結(jié)構(gòu)。證明：如圖所示的線圓結(jié)構(gòu) CA , 0B和圓弧BC之和為最短避障路徑。假設(shè)在平面中有A(a,0)和0(b,0)兩點(diǎn)，中間有一個(gè)半圓形的障礙物，證明從0到B的最路徑為圓弧BC和線段0B、AC的和。平面上連接兩點(diǎn)最短的路徑是通過這兩點(diǎn)的直線段，但是連接兩點(diǎn)的線段被障礙物遮擋，所以設(shè)法嘗試?yán)@過障礙物的折線路徑。在y軸上取一點(diǎn)P(O,y)，若y適當(dāng)大，當(dāng)折線OPA與障礙物相切時(shí)，OPA是這種折線路徑中最短的。由于滿 足0/2的角滿足二:：:tan H所以易知弧度BC小于BPC的長，即證明到了線圓結(jié)構(gòu)為最短避障路徑。O(x1, y1)為起始點(diǎn),線圓

9、結(jié)構(gòu)的長度：A(x2,y2)為目標(biāo)點(diǎn)，D(x3,y3)。求 OCBA，設(shè)為LAO = a22a= (XXi) +(丫2-丫1)OD -bb= ；(xXi)2+(yyi)2AD =c；22c =X3X2y<y2_ ADO = j.2 2 2/b 十 c a、二=arccos()2bcCDO 二-:-二 arccos( b.BDA 二:r二 arcco-sj c因?yàn)?BDC所以：2 二=-?- ：i -從而可得：L =b2 - r2 +、, c2 - r2 +r* s六、模型的建立與求解假設(shè)機(jī)器人從起點(diǎn)R到到目標(biāo)點(diǎn)M。，由圖2知路徑一定是由圓弧和線段組成, 設(shè)有m條線段，n條圓弧。那么最短路

10、程:min S 八"L 亠二 l jr - 10d -10用此模型就可以對(duì)起點(diǎn)到目標(biāo)點(diǎn)之間的路徑進(jìn)行優(yōu)化求解 時(shí)間模型:min T八 ti Pj10LiS.t 二Pvd - 10問題一：1、由以上模型采用窮舉法知從 O > A的路徑線路通過簡(jiǎn)化有如下兩種情況，分 別為，如圖 3 :已知 0（0，0），D（80, 210），A（300，300）圖3用MATLAB件算得：OA的路徑為：471.0372所以：O-A的最短路徑為圖3其結(jié)果為：471.03722、通過窮舉法分析知采用如下圖6所示路線可以可以算出 3B的長度設(shè)D到M點(diǎn)的坐標(biāo)分別為（xi, yi） i= 1.10;Q1 到Q

11、5表示為繞過的各個(gè)障礙物的圓心設(shè)坐標(biāo)為（x ，y ） j=11.15 ; 1j ijnjk表示各直線相交的夾角；p（Xi6，yi6）B（Xi7，yi7）q（Xi8，yi8）類似的采用以上的算法可以得到如下直線的長度：ai=Fbi=JCi=G-Fa2=|p_Eb2 = Q2_Eq=|Q2_Pa3 = |P Jb3 = |Q3-J。二皿-卩a4=|iqb4=lQ4Ic4=Q4qa5 = B-qb5 = Q5-qC5=Q5B0則：夾角為nji=arccos(bj2+cj2-aj22bjcjn j2= arccos()Jbjn j3= arccos()Jcjnji nj2 nj3 nj2_:所以O(shè)B所

12、走路線的,弧線長度為：Lj = nj4r每個(gè)線圓結(jié)構(gòu)起點(diǎn)到終點(diǎn)，起點(diǎn)到圓心，圓心到終點(diǎn)的距離：sSb：r' + Jcjr' + Lj(j=1,2,3,4,5 )故，dB的最短路徑的目標(biāo)函數(shù)為：5minZ 二' sjj = 1通過軟件計(jì)算知：0 > B的最短路徑是869.43323、在通過分析知O-C的最短路徑為圖5所示，通過visio畫圖得到如下圖 所示：設(shè)°i Xj,yj 其中 j 為 1 到 4 的整數(shù)，O(X17,y17)° 的坐標(biāo)分別為(Xi,yi )其中i為5到14的整數(shù)，M(Xl6,yi6).各個(gè)點(diǎn)之間的關(guān)X1 X2X2 X3X?

13、2心_2系：J點(diǎn)坐標(biāo)K 點(diǎn)坐標(biāo)2“ yr和2%廠2G點(diǎn)坐標(biāo)X12 = x3 +rF點(diǎn)坐標(biāo)XX3Z y4yj治我們將°到C分成五個(gè)線圓結(jié)構(gòu)，根據(jù)上述模型分析可以得到:° r A :L5 二 b52 - r2 r 5m > Q : Li,G2- r2 r 十Q > K: L22 -r2, 42- r2r 乜K G : L 32 - r2£2 - r2 r 乜F > C: L4所以：min L汕 L2 L3 L4 L5所以通過MATIAS件編程可以算出：C的最短路徑為:1093.3014、通過分析可知 Of A f B f C f O的最短路徑為如下圖

14、7所示：圖7因?yàn)镃f Af Bf Cf O的路線簡(jiǎn)化成由16個(gè)線圓結(jié)構(gòu)組成的i22FLKMQSf L:f B:f N:f Q：f V:_rr22r rr n4C4 r252r-b6 rc® r第ii頁V2 2 2 2f 丫：1廠七8 一C8 一8T10°C(727.6 , 514)(220,530)(230,530)(140.7,596.3 )(150,444.03 )(222.1 , 460.2 )為了簡(jiǎn)化模型，我們假設(shè) A、B、C點(diǎn)為切點(diǎn) 因?yàn)镺f A和Of C在前面已經(jīng)計(jì)算了最短值 所以min I = OA h 丄 I3 I4 I5 b I7 b I9 ho綜上所述可

15、得各個(gè)點(diǎn)的坐標(biāo)：Of C的坐標(biāo)為：圓弧圓心(230 , 60)(410, 100)(500,200)(720,520)(720,600)切點(diǎn)(422, 90)(428.7, 94.5)(492, 206)(730,520)(730,600) (727.8, 606.3)Of B的坐標(biāo)為： 圓心(60,300)(150,435)(220,470)(150,600)切點(diǎn)(50.13,301.06)(51.7 ,305.5 )(141.7 ,439.6)(225.5 , 538.3)(144.5 , 591.6 )Of A的坐標(biāo)為： 圓心(80,210) 切點(diǎn)(70.5 , 213)(76.6 ,

16、219.4)O > O的坐標(biāo) 圓弧切點(diǎn)(70.5,213.1)(76.6 , 219 )(230,470)291.07,297.78 )(297.25,309.08 )(229.25,532.9)(140.7,596.35 )(225.5,538.35)(144.5,591.64)(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)(435.1 ,671.8

17、 )(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的路線可以得出O > A最短時(shí)間路徑 如圖所示：圖9OCBA是O到A的最短時(shí)間路徑 其中C xi,yi點(diǎn)，B x?, y點(diǎn)分別是直線與圓 弧的兩個(gè)切點(diǎn)：CB長度是dod = J(y2 +% 了 +収2 +為 了直線OlC的斜率為匕，直線OiB的斜率為k2所

18、以直線OlC的方程：yX" (1)直線 O" B 的方程：y-y2 =k2 x-x2 (2)將式子(1)和式子(2)連理求解可以得到圓心坐標(biāo) Oi x,y ,圓弧CB所對(duì)的圓心角為-由到角公式可知：tan二二也 魚 (3)1 +k"k2因?yàn)镺jB _ AB,OiC _ OC ,所以有兩直線垂直那么其斜率相乘等于零。因此：k2 =3°°-X2 (4)300 - y2ki - - x (5)yi在.O" BC中顯然有2r sin d (6)2將式子(3)、(4)、(5)、(6)連理求解可以得到二和r由于轉(zhuǎn)彎的的半徑還受到障礙物 5和障礙物

19、6的影響，所以切點(diǎn)的半徑有限制范 圍，通過計(jì)算可限制切點(diǎn)的半徑范圍：0x1 冬80y1 乞 2900 zx2 乞 290210 乞 y2 乞 290又因?yàn)镈點(diǎn)必須在兩條切線的內(nèi)側(cè)，所以我們通過OC的斜率大于OD;AB的斜率小于AD來限制。即：y_kBA：kDAkDA 0所以最短時(shí)間可以表示為:min T10 -0.1e21 e.300-X2 2 300-y2 2x, %25以上式子用MATLAB求解得到最短時(shí)間為：95.4332秒在上面的模型中是圓心固定算出半徑是 11.5，如果不固定圓心01,那么圓心的范圍就是一個(gè)以0點(diǎn)為圓心，1.5為半徑的小圓內(nèi)部的點(diǎn)。為了方便起見將圓心 01坐標(biāo)的取值定

20、在小圓的內(nèi)切正方形里面。然后利用上面的模型加上（7）式的以在這個(gè)小正方形里面找到最優(yōu)解。d" b （ 7）a = d -b ： c上面式子的程序再加上（7）這個(gè)約束條件用 MATLAB求解得到最短時(shí)間為: 94.3 秒.兩個(gè)切點(diǎn)坐標(biāo) B(85.9,198.6) C(70.1,212.6)七、模型評(píng)價(jià)（一）優(yōu)點(diǎn)：1.文章中的計(jì)算過程皆運(yùn)用數(shù)學(xué)軟件求解，且求解過程簡(jiǎn)潔，使得 出的數(shù)據(jù)更具有說服力；2. 本文通過對(duì)問題的充分分析，在合理的假設(shè)情況下，建立了具有科學(xué)性的方程 模型；3. 運(yùn)用本文模型求解相關(guān)問題時(shí)結(jié)果與實(shí)際情況基本相吻合；（二）缺點(diǎn)：?jiǎn)栴}（一）中其他幾問的算法基本上同 OA的

21、算法一致，數(shù)據(jù)較 大，在處理時(shí)存在一定的誤差，但誤差在允許的范圍內(nèi)，可以接受。八、模型檢驗(yàn)與推廣本文是利用傳統(tǒng)方法一一切線圖法求解切線圖法用障礙物的切線表示弧，此 時(shí)移動(dòng)機(jī)器人必須接近障礙物，在有誤差的時(shí)候可能與障礙物有接觸， 但解決了 障礙物不能是圓形的問題。我們可以采用基于遺傳算法與人工神經(jīng)網(wǎng)絡(luò)的智能避 障算法。九、參考文獻(xiàn)1 ，2012 年2 陽明盛，熊西文，林建華，MATLABS礎(chǔ)及數(shù)學(xué)軟件，大連：大連理工大學(xué)出 版社，2003年。3 移動(dòng)機(jī)器人避障方法綜述作者牢常健，吳成東，李斌科學(xué)院沈陽動(dòng)化研究所機(jī)器人學(xué)田家重點(diǎn)實(shí)驗(yàn)室；中嗣科學(xué)院研究生院；東北大學(xué)信息科學(xué)與工程學(xué)院十，附錄附錄一一

22、 ;為最短路線OA的程序，是利用MATIAB求解的%0 :初始點(diǎn) D:轉(zhuǎn)彎圓弧圓心 A:到達(dá)點(diǎn) r:表示圓弧半徑function result=z on gcha ng1(r)0(1)=0;0(2)=0;A(1)=300;A(2)=300;D(1)=80;D(2)=210;OD = sqrt(O(1)-D(1)A2+( 0( 2)-D(2)A2);0A=sqrt(0(1)-A(1)A2+(0 (2)-A(2)F2);AD=sqrt(D(1)-A(1)A2+(D(2)-A(2)A2);alpha仁acos(0DA2+ADA2-0AA2)/(2*0D*AD);alpha2 = acos(r/0D)

23、;alpha3 = acos(r/AD);alpha4=2*pi-alpha1-alpha2-alpha3;%alpha4 為轉(zhuǎn)彎圓心角0S仁sqrt(0DA2-rA2);%0S1,AS2 均為圓弧切線 %S2A=sqrt(ADA2-rA2);S1S2hu=r*alpha4;result=0S1+S1S2hu+S2A;endzon gcha ng1(10)ans =471.0372附錄二；為最短路線O- B的程序，是利用MATIAB編程的0-F%求解一次轉(zhuǎn)彎所經(jīng)路線總長%0 :初始點(diǎn) Q1:轉(zhuǎn)彎圓弧圓心 F:到達(dá)點(diǎn)r:表示圓弧半徑 pfunction result=z on gcha ngob

24、1(r)0(1)=0;0(2)=0;F(1)=141.675;F (2)=440.55;Q1(1)=60;Q1(2)=300;0Q1 = sqrt(0(1)-Q1(1)A2+(0 (2)-Q1(2)A2);0F=sqrt(0(1)-F(1)A2+( 0( 2)-F(2)A2);FQ仁 sqrt(Q1(1)-F(1)A2+(Q1(2)-F(2)A2);alpha仁acos(0Q1A2+FQ1A2-0FA2)/(2*0Q1*FQ1);alpha2 = acos(r/0Q1);alpha3 = acos(r/FQ1);alpha4=2*pi-alpha1-alpha2-alpha3;%alpha4

25、為轉(zhuǎn)彎圓心角OD=sqrt(OQ1A2-rA2);%OD,FE 均為圓弧切線 % EF=sqrt(FQ1A2-rA2);DEhu=r*alpha4;result=OD+DEhu+EF;endzon gcha ngobl(IO)F-P%求解一次轉(zhuǎn)彎所經(jīng)路線總長% E：初始點(diǎn) Q2:轉(zhuǎn)彎圓弧圓心P:到達(dá)點(diǎn)r:表示圓弧半徑 pfunction result=z on gcha ngob1(r)E(1)=51.675;E (2)=305.5;P(1)=185;P(2)=452.5;Q2(1)=150;Q2(2)=435;EQ2 = sqrt(E(1)-Q2(1)A2+(E (2)-Q2(2)F2);E

26、P=sqrt(E(1)-P(1)A2+(E (2)-P(2)A2);PQ2=sqrt(Q2(1)-P(1)A2+(Q2 (2)-P(2)A2);alpha仁acos(EQ2A2+PQ2A2-EPA2)/(2*EQ2*PQ2);alpha2 = acos(r/EQ2);alpha3 = acos(r/PQ2);alpha4=2*pi-alpha1-alpha2-alpha3;%alpha4 為轉(zhuǎn)彎圓心角EF=sqrt(EQ2A2-rA2);%EF,PG 均為圓弧切線 %PG=sqrt(PQ2A2-rA2);FGhu=r*alpha4;result=FGhu+PG;endP-J%求解一次轉(zhuǎn)彎所經(jīng)路

27、線總長% P:初始點(diǎn)Q3:轉(zhuǎn)彎圓弧圓心 J:到達(dá)點(diǎn)r:表示圓弧半徑 pfunction result=z on gcha ngob1(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)A2+(P (2)-Q3(2)F2);JP=sqrt(J(1)-P(1)A2+(J(2)-P(2)A2);JQ3=sqrt(Q3(1)-J(1)A2+(Q3(2)-J(2)A2);alpha仁acos(JQ3A2+PQ3A2-JPA2)/(2*JQ3*PQ3);alpha2 = acos(r/PQ3)

28、;alpha3 = acos(r/JQ3); alpha4=2*pi-alpha1-alpha2-alpha3;%alpha4 為轉(zhuǎn)彎圓心角 JI=sqrt(JQ3A2-rA2);%JI,PH 均為圓弧切線 % PH=sqrt(PQ3A2-rA2);Hlhu=r*alpha4;result=PH+Hlhu+JI;endJ-q%求解一次轉(zhuǎn)彎所經(jīng)路線總長% P:初始點(diǎn)Q3:轉(zhuǎn)彎圓弧圓心 J:到達(dá)點(diǎn) r:表示圓弧半徑 pfunction result=z on gcha ngob1(r)J(1)=230;J (2)=530;P(1)=185;P(2)=452.5;Q3(1)=220;Q3(2)=47

29、0;PQ3 = sqrt(P(1)-Q3(1)A2+(P (2)-Q3(2)F2);JP=sqrt(J(1)-P(1)A2+(J(2)-P(2)A2);JQ3=sqrt(Q3(1)-J(1)A2+(Q3(2)-J(2)A2);alpha仁acos(JQ3A2+PQ3A2-JPA2)/(2*JQ3*PQ3);alpha2 = acos(r/PQ3);alpha3 = acos(r/JQ3);alpha4=2*pi-alpha1-alpha2-alpha3;%alpha4 為轉(zhuǎn)彎圓心角JI=sqrt(JQ3A2-rA2);%JI,PH 均為圓弧切線 %PH=sqrt(PQ3A2-rA2);Hlhu

30、=r*alpha4;result=PH+HIhu+JI;endq-B%求解一次轉(zhuǎn)彎所經(jīng)路線總長% q :初始點(diǎn) Q5:轉(zhuǎn)彎圓弧圓心B:到達(dá)點(diǎn)r:表示圓弧半徑pfunction result=z on gcha ngob1(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)A2+(B(2)-Q5(2)A2);Bq=sqrt(B(1)-q(1)A2+(B(2)-q(2)A2); qQ5=sqrt(Q5(1)-q(1)A2+(Q5 (2)-q(2)A2);alpha仁acos(BQ5A2+qQ5

31、A2-BqA2)/(2*BQ5*qQ5);alpha2 = acos(r/qQ5);alpha3 = acos(r/BQ5);alpha4=2*pi-alpha1-alpha2-alpha3;%alpha4 為轉(zhuǎn)彎圓心角BM=sqrt(BQ5A2-rA2);%BM,qL 均為圓弧切線 % qL=sqrt(qQ5A2-rA2);LMhu=r*alpha4;result=qL+LMhu+BM;endfun ctio n oc (a,b,c,d,e);sum5=zon gcha ngob0F(10)+z on gcha ngobFP(10)+z on gcha ngobPJ(10)+z on gch

32、a ngobJq(10)+z on gcha ngo bqB(10)869.4896附錄三；為最短時(shí)間路線O-A的程序(圓心固定時(shí))，是利用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)A2+(E (2)-F(2)A2);%bEG=sqrt(E(1)-G(1)A2+(E (2)-G(2)F2);%aFG=sqrt(F(1)-G(1)A2+(F (2)-G(2)A

33、2);%calpha仁acos(EFA2+FGA2-EGA2)/(2*EF*FG);% 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(EFA2-r(i)A2);%ES1,ES2 均為圓弧切線 %S2G=sqrt(FGA2-r(i)A2);S1S2hu=r(i)*alpha4;L(i)=ES1+S1S2hu+S2G;% 總路程a(i)=ES1/5+S2G/5+S1S2hu*(1+exp(10-0.1*r(i)