2012年高教社杯數(shù)學(xué)建模D題--機(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á)最終目標(biāo)點(diǎn)的多 種情形，尋找出一條恰當(dāng)?shù)膹慕o出發(fā)點(diǎn)到目標(biāo)點(diǎn)的運(yùn)動(dòng)路徑使機(jī)器人在運(yùn)動(dòng)中能安全、無碰撞的 繞過障礙物而使用的路徑和時(shí)間最短。由于規(guī)定機(jī)器人的行走路徑由直線段和圓弧組成，其中圓 弧是機(jī)器人轉(zhuǎn)彎路徑，機(jī)器人不能折線轉(zhuǎn)彎。所以只要給定的出發(fā)點(diǎn)到目標(biāo)點(diǎn)存在至少一個(gè)障礙 物，我們都可以認(rèn)為最短路徑一定是由線和圓弧所組成，因此我們建立了切線圓結(jié)構(gòu)，這樣無論 路徑多么復(fù)雜，我們都可以將路徑劃分為若干個(gè)這種切線圓結(jié)構(gòu)來求解。

2、在沒有危險(xiǎn)碰撞的情況 下，圓弧的半徑越小，路徑應(yīng)該越短，因此我們盡量選擇最小的圓弧半徑以達(dá)到最優(yōu)。對(duì)于途中 經(jīng)過節(jié)點(diǎn)的再到達(dá)目標(biāo)點(diǎn)的狀況，我們采用了兩種方案，一種是在拐點(diǎn)和節(jié)點(diǎn)都采用最小轉(zhuǎn)彎半 徑的形式，另一種是適當(dāng)擴(kuò)大拐點(diǎn)處的轉(zhuǎn)彎半徑，使得機(jī)器人能夠沿直線通過途中的目標(biāo)點(diǎn)。然 后建立了最優(yōu)化模型對(duì)兩種方案分別進(jìn)行求解，把可能路徑的最短路徑采用窮舉法列舉出來，用 lingo 工具箱求解得出了機(jī)器人從 O(0,0) 出發(fā)， OA、OB、OC 和 OABCO 的最短路 徑；利用 matlab 中的 fminbnd 函數(shù)求極值的方法求出了機(jī)器人從 O(0,0) 出發(fā)，到達(dá) A 的最短時(shí)間 路徑。本文

3、提出一種最短切線圓路徑的規(guī)劃方法，其涉及的理論并不高深，只是應(yīng)用了幾何知識(shí) 和計(jì)算機(jī)程序、數(shù)學(xué)工具計(jì)算，計(jì)算簡(jiǎn)易，便于實(shí)現(xiàn)，能搞提高運(yùn)行效率。問題一OA最短路徑為： LOA =471.0372OB最短路徑為： LOB1 853.8014OC最短路徑為： LOC4=1054.0OABCO 最短路徑為：?jiǎn)栴}二機(jī)器人從 O(0,0) 出發(fā)，到達(dá) A 的最短時(shí)間路徑：最短時(shí)間是 94.5649 ，圓弧的半徑是 11.5035, 路徑長 LOA 472.4078關(guān)鍵詞 最短路徑；避障路徑；最優(yōu)化模型；解析幾何；數(shù)學(xué)工具一、問題重述圖1是一個(gè) 800800的平面場(chǎng)景圖，在原點(diǎn) O(0,0) 點(diǎn)處有一個(gè)機(jī)器

4、人，它只能在該平面場(chǎng)景范 圍內(nèi)活動(dòng)。圖中有 12個(gè)不同形狀的區(qū)域是機(jī)器人不能與之發(fā)生碰撞的障礙物，障礙物的數(shù)學(xué)描述 如下表：編號(hào)障礙物名 稱左下頂點(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,

5、600)底邊長 90，左上頂點(diǎn)坐標(biāo) (180,680)9長方形(370,680)長60，寬12010正方形(540,600)邊長13011正方形(640,520)邊長8012長方形(500,140)長300，寬60在圖1的平面場(chǎng)景中，障礙物外指定一點(diǎn)為機(jī)器人要到達(dá)的目標(biāo)點(diǎn)(要求目標(biāo)點(diǎn)與障礙物的距 離至少超過 10個(gè)單位)。規(guī)定機(jī)器人的行走路徑由直線段和圓弧組成，其中圓弧是機(jī)器人轉(zhuǎn)彎路 徑。機(jī)器人不能折線轉(zhuǎn)彎，轉(zhuǎn)彎路徑由與直線路徑相切的一段圓弧組成，也可以由兩個(gè)或多個(gè)相 切的圓弧路徑組成，但每個(gè)圓弧的半徑最小為 10個(gè)單位。為了不與障礙物發(fā)生碰撞，同時(shí)要求機(jī) 器人行走線路與障礙物間的最近距離為

6、10個(gè)單位，否則將發(fā)生碰撞，若碰撞發(fā)生，則機(jī)器人無法 完成行走。機(jī)器人直線行走的最大速度為 v0 5個(gè)單位/ 秒。機(jī)器人轉(zhuǎn)彎時(shí)，最大轉(zhuǎn)彎速度為v v( )1v00 0.1 2 ，其中 是轉(zhuǎn)彎半徑。如果超過該速度，機(jī)器人將發(fā)生側(cè)1 e10 0.1 翻，無法完成行走。請(qǐng)建立機(jī)器人從區(qū)域中一點(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ì)算：(1) 機(jī)器人從O(0,0) 出發(fā)，OA、OB、OC和OABCO的最短路徑。(2) 機(jī)器人從O(0,0) 出發(fā)，到達(dá)A的最短時(shí)間路徑。 注：要

7、給出路徑中每段直線段或圓弧的起點(diǎn)和終點(diǎn)坐標(biāo)、圓弧的圓心坐標(biāo)以及機(jī)器人行走的總距 離和總時(shí)間。圖1800800平面場(chǎng)景圖問題分析1、要求求定點(diǎn) O(0,0) 按照一定的行走規(guī)則繞過障礙物到達(dá)目標(biāo)點(diǎn)的最短路徑，我們先可以包絡(luò) 線畫出機(jī)器人行走的危險(xiǎn)區(qū)域，在沒有危險(xiǎn)碰撞的情況下，圓弧的半徑越小，路徑應(yīng)該越短。 這樣的話，拐角處就是一個(gè)以方形或三角形障礙物的頂點(diǎn)為圓心半徑為10 個(gè)單位的圓弧，如果是圓形障礙物就應(yīng)該是以障礙物的圓心為圓心、障礙物的半徑長加上10 為半徑的圓弧。2、若經(jīng)過中間的若干點(diǎn)按照一定的規(guī)則繞過障礙物到達(dá)目標(biāo)點(diǎn)，這使我們考慮就不僅僅是經(jīng)過障 礙物拐點(diǎn)的問題，也應(yīng)該考慮經(jīng)過路徑中的目

8、標(biāo)點(diǎn)處轉(zhuǎn)彎的問題，這時(shí)簡(jiǎn)單的線圓結(jié)構(gòu)就不能 解決這種問題，我們?cè)诠拯c(diǎn)及途中目標(biāo)點(diǎn)處都采用最小轉(zhuǎn)彎半徑的形式，也可以適當(dāng)?shù)淖儞Q拐 點(diǎn)處的拐彎半徑，使機(jī)器人能夠沿直線通過途中的目標(biāo)點(diǎn)，然后建立優(yōu)化模型對(duì)這兩種方案分 別進(jìn)行優(yōu)化，最終求得最短路徑。3、這樣機(jī)器人行走的路徑就是由切線段、內(nèi)公切線、外公切線以及圓弧組成的這里稱之為切線圓 路徑。三、 模型假設(shè)1、假設(shè)障礙物只包含長方形、正方形、三角形、圓形。2、假設(shè)機(jī)器人能夠抽象成點(diǎn)來處理。3、路徑不考慮走平面場(chǎng)地的邊界。五、符號(hào)說明在計(jì)算機(jī)程序輸入的原始數(shù)據(jù)中 :(T,V,W,r) 表示 T 是起點(diǎn)坐標(biāo) ,V 是圓弧的圓心坐標(biāo) ,W 是目標(biāo) 節(jié)點(diǎn)坐標(biāo),

9、r 是圓弧半徑 .為便于敘述和計(jì)算，根據(jù)已知條件我們給 12個(gè)障礙物中的 11 個(gè)方形和三角形頂點(diǎn)用字母 Tij 或 Sij 表示，其中 T 或 S 表示障礙物， Tij 中 i 表示第 i 號(hào)障礙物， Sij 中 i 表示第 9+i 號(hào)障礙物， j 表示從左下角開始按順時(shí)針數(shù)起第幾個(gè)頂點(diǎn)。如下表一所示：表一編號(hào)障礙物名 稱左下頂點(diǎn)坐 標(biāo)左上頂點(diǎn)坐標(biāo)右上頂點(diǎn)坐標(biāo)右下頂點(diǎn)坐 標(biāo)1正方形 T1T11(300,400T12(300，T13(500，T14(500，)600)600)400)2圓形T2圓心坐標(biāo) T2(550,450) ，半徑 703平行四邊T31(360,240T32( 400，T33

10、(540，T34(500，形T3)330)330)240)4三角形 T4T41(280,100T42(345，T43(410，)210)100)5正方形 T5T51(80,60)T52(80，T53(230，T54(230，210)210)60)6三角形 T6T61(60,300)T62(150，T64(235，435)300)7長方形 T7T71(0,470)T72(0，T73(220，T74(220，530)530)470)8平行四邊T81(150,600T82(180，T83(270，T84(240，形T8)680)680)600)9長方形 T9T91(370,680T92(370，T9

11、3(430，T94(430，)800)800)680)10正方形 S1S11(540,600S12(540，S13(670，S14(670，)730)730)600)11正方形 S2S21(640,520S22(640，S23(720，S24(720，)600)600)520)12長方形 S3S31(500,140S32(500，S33(800，S34(800，)200)200)140)注：各個(gè) 障礙物頂點(diǎn)如 需要設(shè)計(jì)圓 弧，粗略表示 路徑時(shí)圓弧的 位置簡(jiǎn)單用障 礙物頂點(diǎn)字母 表示(如：O T61 T62表示從點(diǎn) O經(jīng) 過 6 號(hào)三角形 左頂角為圓心 的圓弧到 6 號(hào) 三角形上頂角為圓心的圓弧

12、到 7號(hào)方形右下角為圓心的圓弧到 7 號(hào)方形右上角為圓心的圓弧到 8號(hào)菱形左下角為 圓心的圓弧到達(dá)點(diǎn) B。標(biāo)出的經(jīng)過頂點(diǎn)都是需要設(shè)計(jì)圓弧的)符號(hào)說明路徑的總長度第i 段切線的長度第 j 段圓弧的長度轉(zhuǎn)彎半徑障礙物上的任意點(diǎn)與行走路徑之間的最短距離以頂點(diǎn) Tij 或 Sij 為圓心的圓弧的兩個(gè)切點(diǎn)路徑時(shí)間六、模型的建立與求解由于規(guī)定機(jī)器人的行走路徑由直線段和圓弧組成，其中圓弧是機(jī)器人轉(zhuǎn)彎路徑，機(jī)器人不能折 線轉(zhuǎn)彎。據(jù)此可以這樣認(rèn)為，起點(diǎn)到目標(biāo)點(diǎn)無論中間障礙物有多少，最短路徑都應(yīng)該是若干個(gè)切 線圓結(jié)構(gòu)所組成。易知，求兩點(diǎn)之間的最短路徑中的轉(zhuǎn)彎半徑越小路徑就越短，我們應(yīng)該按照最 小的轉(zhuǎn)彎半徑來算才能

13、達(dá)到最優(yōu)。根據(jù)要求機(jī)器人行走線路與障礙物間的最近距離為10 個(gè)單位，因此在方形及三角形頂點(diǎn)轉(zhuǎn)彎的地方圓弧半徑 r 10 ，我們盡量取以頂點(diǎn)為圓心半徑為 r=10 個(gè)單 位的圓弧，如果是圓形障礙物就應(yīng)該是以障礙物的圓心為圓心、障礙物的半徑長加上10 為半徑的圓弧，只有在必要的時(shí)候?qū)Π霃阶鬟m當(dāng)?shù)募哟笳{(diào)整。6.1 模型 求從起點(diǎn) O(0,0) 到目標(biāo)點(diǎn) A(300,300) 的最短路徑 。經(jīng)過觀察很顯然從 O到 A有兩條選擇的路徑(其它路徑需要經(jīng)過過多的障礙物路徑顯然比較長不必考慮)如圖 6.11 所示，一條是從 5 號(hào)障礙物的左上角走 OPQA ，一條是從右下角走 OHJA 。他們的路徑結(jié)構(gòu)圖是類

14、似的如圖 6.12 所示圖 6.11圖 6.12于隔離危險(xiǎn)線拐角小圓弧的切點(diǎn)，圓心為 T ( x5, y5 ) ,圓的半徑為 r ，OA的長度為 a ， OT的長度為b ， AT的長度為 c，角度 OTA=, OTAOTP , ATQ , PTQ . 求的長度，設(shè)為L.解法如下：如上圖可得有以下關(guān)系：在 OTA中 ：OA 在 Rt OTP ： 在 Rt OTP 中： 所以： 從而可得路徑長：arccos(b22c2bca2r1.1)Lb2 r 2即模型 為： min L b rr又由 (x5 x1)2(y5 y1)2 102(x5 x3)2 (y5(x3 x1)2 (y322y3)2 r 2y

15、1)2(1)= arccos由已知條件知 O(0，0), A(300，300),T (80,210) 即x1 0, y1 0, x2 300,y2 300,x5 80,y5 210，r 10.利用計(jì)算機(jī)計(jì)算 lingo 算法(T 是起點(diǎn)坐標(biāo) V是圓弧圓心坐標(biāo) W是過程目標(biāo)點(diǎn)坐標(biāo) r 圓弧是半徑 ) ：functionresult=zongchang(T,V,W,r)TV=sqrt(T(1)-V(1)2+(T(2)-V(2)2);TW=sqrt(T(1)-W(1)2+(T(2)-W(2)2);VW=sqrt(V(1)-W(1)2+(V(2)-W(2)2); alpha1=acos(TV2+VW2

16、-TW2)/(2*TV*VW); alpha2=acos(r/TV);alpha3=acos(r/VW);alpha4=2*pi-alpha1-alpha2-alpha3;TS1=sqrt(TV2-r2);S2W=sqrt(VW2-r2);S1S2hu=r*alpha4;result=TS1+S1S2hu+S2W;結(jié)果算得 的長度 LOA =471.0372和 (x5-x2)2y3)2(2)2 2 2(y5 y2)2 102 (x5 x4)2 (y52 2 2(x5 x4) (y5 y4) r計(jì)算得點(diǎn) T52為圓心的圓弧兩切點(diǎn)的坐標(biāo)為 P52(70.506, 213.1406), Q52 (7

17、6.6064, 219.4066) 。 同樣，通過計(jì)算可得從路徑從 右下角方向(圓心 T的坐標(biāo)改為 5 號(hào)障礙物右下角頂點(diǎn)坐標(biāo) T54 (230,60 )走時(shí) OHJA長度 LOA 498.4259.很顯然機(jī)器人從 5號(hào)障礙物左上角 T52走的路徑 OPQA小于機(jī)器人 5號(hào)障礙物右下角 T54 走的路 徑 OHJA 要短。因此從起點(diǎn) O(0,0) 到目標(biāo)點(diǎn) A(300,300) 的最短路徑是，長度 LOA=471.0372，圓弧PQ的切點(diǎn)坐標(biāo)為 P(70.506,213.1406), Q(76.6064,219.4066) 。同時(shí)可以驗(yàn)證 6 號(hào)三角形障礙物右下角頂點(diǎn)到切線 AQ距離大于 10

18、，所以過 T52的OPQA 路徑是安全的也是最短的。下面考慮問題二：從 O到 A 的最短時(shí)間路徑由式( 1.1 )可求得時(shí)間路徑的目標(biāo)函數(shù)為：mint b2 r 2 5 c2 r2 5 r150 0.1r2 ，80 t 1010 0.1r1e利用 matlab 中的 fminbnd 函數(shù)求極值 f1=(x*(2*pi-2.3231-acos(x/224.7221)-acos(x/237.6973)/(5/(1+exp(10-0.1*x2)+sqrt( 224.72212-x2)/5+sqrt(237.69732-x2)/5;x_min,f_min,flag=fminbnd(f1,10,50)

19、得結(jié)果：x_min=11.5035 ，f_min=94.5649 ， flag=1即最短時(shí)間是 94.5649 ，T52圓弧的半徑是 11.5035 。利用式( 1)( 2)算得兩切點(diǎn)坐標(biāo)P52 (69.0546，213.5395), Q52 (76.1648，220.8453) ,路徑長 LOA 472.4078 .6.2 模型 求從起點(diǎn) O(0,0) 到目標(biāo)點(diǎn) B(100,700) 的最短路徑 。根據(jù)障礙物的形狀與位置，要從點(diǎn) O(0,0) 到點(diǎn) B(100,700) ，經(jīng)過觀察我們給出了可能的 4 條路徑 的最短路徑:圖6.21(1) 所示標(biāo)出了 3種路徑，路徑 OB1：從點(diǎn) O 6號(hào)三

20、角形左頂角 6號(hào)三角形 上頂角 7號(hào)方形右下角 7號(hào)方形右上角 8號(hào)菱形左下角 點(diǎn) B(簡(jiǎn)寫成OB1:O T61 T62 T74 T73 T81 B ，經(jīng)過各個(gè)障礙物頂點(diǎn)時(shí)都設(shè)計(jì)圓弧，這里圓弧的位置 簡(jiǎn)單用頂點(diǎn)字母表示 , 以下同)，路徑 OB2：O T54 T63 T73 T81 B, 路徑 OB3：O T52 T63 T73 T81 B; 圖 6.21(2) 所示標(biāo)出了 1 種路徑 , 路徑 OB4：O T54 T73 T81 B ,這條路徑是為了使 T52 (取半徑為 r=15)到T73 (取半徑為 r=25) 的路徑成 為直線增大了圓弧半徑而考慮的 . 我們可以分別計(jì)算出 4條可能路徑

21、的最短路徑的長度，然后進(jìn)行 比較，取最小者就是 O到目標(biāo)點(diǎn) B 的最優(yōu)路徑。圖 6.21（1）圖 6.21（2） 分析：從路徑觀察，求解過程中我們會(huì)遇到 以下三種情況，我們不能直接采用切線圓 6.11（ 2） 的結(jié)構(gòu)來解決，需要做簡(jiǎn)單的變換。情況一：機(jī)器人經(jīng)過 6 號(hào)三角形障礙物上頂點(diǎn) T62 （150,435 ）和 7號(hào)方形障礙物右下頂點(diǎn) T74 （220,470 ）之間的路徑以及 7 號(hào)方形障礙物右上頂點(diǎn) T73 （220,530 ）和 8號(hào)障礙物左下頂點(diǎn) T81 （150,600 ）之間的路徑時(shí)，其路徑如下圖 6.22 所示：6.22我們假設(shè)兩圓心坐標(biāo)分別為 O（x1,y1）和O （x2

22、, y2） ,半徑均為 r ， M點(diǎn)坐標(biāo)為 （ x3 , y3） ，那么我們很容 易可以求得 :這樣我們就可以利用 1）中的方法，先求 A到M，再求 M到B，這樣將上圖分割成兩個(gè) 6.11（2）所 示的線圓結(jié)構(gòu)，分兩段就可以求解。情況二：機(jī)器人經(jīng)過 6號(hào)三角形障礙物的左下頂點(diǎn) T61 （60,300 ）到上頂點(diǎn) T62（150.435）和經(jīng)過 7 號(hào)方形障礙物右下頂點(diǎn) T74 （220,470 ）到右上頂點(diǎn) T73 （220,530 ）時(shí)路徑如下圖 6.12 （3）所 示：圖 6.23這里我們依然設(shè)圓心坐標(biāo)分別為 O（x1,y1）和O （x2,y2）,半徑均為 r ，這樣我們可以得到：那么OO

23、 直線方程為：因?yàn)楣芯€ DE與 OO 平行，那么 DE的直線方程可以表示為： 其中：那么把公切線的方程與圓的方程聯(lián)立，就可以求得切點(diǎn) D和 E的坐標(biāo)。這樣用 D和E任意一點(diǎn)作為 分割點(diǎn)都可以將上圖分割成兩個(gè) 6.12 所示的線圓結(jié)構(gòu)，這樣就可以對(duì)其進(jìn)行求解。同理多個(gè)這樣 的轉(zhuǎn)彎時(shí)，用同樣的方法可以進(jìn)行分割。情況三，兩切線圓的半徑不相同如下圖 6.24 所示，只要在 6.23 的基礎(chǔ)上作適當(dāng)?shù)恼{(diào)整即可。圖 6.24圖 6.24 中大圓弧小圓弧的切點(diǎn)坐標(biāo)就不能用式（ 1 ）（ 2）來計(jì)算，為此我們?cè)O(shè)大圓的圓心O6（x1,x2），半徑為 R,小圓的圓心 O5（x2,y2） ,半徑為 r, 切點(diǎn)坐標(biāo)

24、為（ x0 , y0 ） ，則大圓：(x0 x1)2 ( y0y1)2R2(x0x2)2 (x0y2)2(x1 x2) 2 (y1y2)2 (R r)2 r 2(3)小圓：（x0 x1）2 （y0 y1）2 （x1 x2）2 （y1 y2）2 （R r）2 R2 （4）利用（ 3）式和（ 4）式就可以算出大圓弧和小圓弧的切點(diǎn)的坐標(biāo)。顯然在路徑中不管是出現(xiàn)這三種情況中的那種情形，都可以將圖形分割成兩個(gè) 6.12 所示的 線圓結(jié)構(gòu)來解出路徑的長。因此我們把圖 6.12 中的模型 的一個(gè)完整線圓結(jié)構(gòu)路徑長（兩條切線 連一段圓弧）稱為一個(gè)子任務(wù)，那么假設(shè)機(jī)器人從起點(diǎn) O到目標(biāo)點(diǎn) M ，由分析知路徑一定

25、是由圓 弧和線段組成，就可以看成若干個(gè)子任務(wù)的和，設(shè)從O到M 路徑中有 n 個(gè)障礙物是過程目標(biāo)節(jié)點(diǎn)，那么整個(gè)路徑長就有 n 個(gè)子任務(wù)，那么目標(biāo)函數(shù)可以表示為：Li表示第i 個(gè)任務(wù)的路徑長。用此模型就可以對(duì)起點(diǎn)到目標(biāo)點(diǎn)之間的路徑進(jìn)行優(yōu)化求解,對(duì)計(jì)算過程中重復(fù)計(jì)算的一些切線要減去。這樣利用計(jì)算機(jī)運(yùn)算時(shí)只需重復(fù)利用上面的 lingo 算法然 后求和就可以算出結(jié)果 , 非常簡(jiǎn)便。根據(jù)上述分析求路徑 OB1:O T61 T62 T74 T73 T81 B ，取各頂點(diǎn)圓弧的半徑相同均 為 r 10 ，路徑中包含了 6.12 切線圓、情況一內(nèi)公切線和情況二外公切線三種情形；由下面 lingo 算法 func

26、tionresult=zongchang(T,V,W,r) TV=sqrt(T(1)-V(1)2+(T(2)-V(2)2);TW=sqrt(T(1)-W(1)2+(T(2)-W(2)2); VW=sqrt(V(1)-W(1)2+(V(2)-W(2)2); alpha1=acos(TV2+VW2-TW2)/(2*TV*VW);alpha2=acos(r/TV); alpha3=acos(r/VW); alpha4=2*pi-alpha1-alpha2-alpha3;TS1=sqrt(TV2-r2); S2W=sqrt(VW2-r2);S1S2hu=r*alpha4; result=TS1+S1S

27、2hu+S2W; functionresult=ltt(T,V,W,r) TV=sqrt(T(1)-V(1)2+(T(2)-V(2)2);TW=sqrt(T(1)-W(1)2+(T(2)-W(2)2); VW=sqrt(V(1)-W(1)2+(V(2)-W(2)2); alpha1=acos(TV2+VW2-TW2)/(2*TV*VW); alpha2=acos(r/TV);alpha3=90*pi/180; alpha4=2*pi-alpha1-alpha2-alpha3;%alpha4 為轉(zhuǎn)彎圓心角 TS1=sqrt(TV2-r2);%TS1,TS2 均為圓弧切線 %S2W=VW; S1S

28、2hu=r*alpha4; result=TS1+S1S2hu+S2W; 結(jié)果:) ：可算得得LOB4 958.8002得路徑 OB1的長為 : LOB1 853.8014.路徑 OB2:OT54T63T73T81 B 的長可算得得 LOB2 945.9491路徑OB3:OT52T63T73T81 B 可算得得 LOB3 917.7443路徑OB4:OT54T73T81B T52（ T52 取半徑為 r=15, T73取半徑為 r=25, 半徑不能取為 10，是因?yàn)楸苊馀c 6 號(hào)三角形右下頂角的位置產(chǎn)生碰撞B (r=10) ，長 LOB1 853.8014可以看出 , 最短路徑為 O T61

29、T62 T74 T73 T81面由式（ 1）（ 2）計(jì)算最短路徑 O T61 T62 T74 T73 T81 B中 5段圓弧的切點(diǎn)坐標(biāo)得：圓弧圓心切點(diǎn)坐標(biāo)切點(diǎn)坐標(biāo)半徑 r=10以 T61 為圓心以T62 為圓心以 T74 為圓心以 T73 為圓心以 T81 為圓心6.3 模型 求從起點(diǎn) O(0,0) 到目標(biāo)點(diǎn) C(700,640) 的最短路徑。無論路徑多么復(fù)雜，根據(jù)上面的分析我們都可以將路徑劃分為若干個(gè)這種切線圓結(jié)構(gòu)來求 解。因此求從起點(diǎn) O(0,0) 到目標(biāo)點(diǎn) C(700,640) 的最短路徑，目標(biāo)函數(shù)與模型 一樣可以表示為： 表示機(jī)器人從起點(diǎn) O到到目標(biāo)點(diǎn) M，要完成 n個(gè)子任務(wù)， Li表

30、示第 i 個(gè)任務(wù)的路徑長(兩條切線 連一段圓弧)。設(shè)從 O到M 路徑中有 n個(gè)障礙物是過程目標(biāo)節(jié)點(diǎn)，那么整個(gè)路徑長就有 n個(gè)子任 務(wù)，其和即為整個(gè)路徑長。如下圖 6.31 我們給出了從點(diǎn) O到點(diǎn) C的4條路徑 O T52 T34 S24 S23 C.OT54 T43S32S24S23C,OT52 T32T2S24S23C(這里T2是 2號(hào)圓形障礙物的同心圓，半徑取r=70+10)O T54 T32 T2由 lingo 算法得：S24S23C(這里T2是 2號(hào)圓形障礙物的同心圓，半徑取圖 6.31r=70+10)路徑OC1:OT52T34S24S23 C ，路徑長 LOC1=1.1362e+00

31、3=1136.2路徑OC2:OT54T43S32S24 S23 C ,路徑長 LOC2 =1.0854+003=1085.4路徑OC3:OT52T32T2S24 S23 C, 路徑長 LOC3 =1.0639e+003=1063.9路徑OC4:OT54T32T2S24 S23 C, 路徑長 LOC4 =1.0540e+003=1054.0可以看出路徑 OC4:O T54 T32 T2 S24 S23 C, 路徑長 LOC 4 =1054.0 是最短的 .下面由式( 1)(2)計(jì)算最短路徑 O T54 T32 T2 S24 S23 C 中 5 段圓弧的切點(diǎn)坐標(biāo)得：圓弧圓心切點(diǎn)坐標(biāo)切點(diǎn)坐標(biāo)半徑T5

32、4為圓心P54 (232.1149 ，50.2262)Q54 (239.5323 ，56.9777)r=10T32為圓心P32 (395.2092,338.7777)Q32 (398.1397 ，339.8254)r=10T2 為圓心P2 (476.5806 ，481.7742)Q2 (606.3314 ，393.1953)r=80s24 為圓心P24 (727.0414 ，512.8994)Q24 (730,520)r=10s23為圓心P23 (730,580)Q23 (729.4868,583.1623)r=10驗(yàn)證此路徑跟沿途其它障礙物的安全性 ,T 41(280,100),T 31(3

33、60,240) 到T54 和T32之間的圓切線的距 離為 10,故此路徑是切實(shí)可行也是最短的路徑 .6.4 模型 V 求機(jī)器人從 O A B CO的最短路徑。CO中的路段可以采用 S23 S24機(jī)器人要經(jīng)過點(diǎn) A、B、 C，這些點(diǎn)要么在路徑的切直線段上(根據(jù)三點(diǎn)所處的位置這顯然不可能)，那么只能在 A、B、C 三點(diǎn)分別設(shè)立圓弧使得這三點(diǎn)分別在圓弧上，這樣就必須確定三圓弧的圓心 。由于 OC的最優(yōu)路徑已經(jīng)由模型 III 求出為 O T54 T32 T2 S24 S23 C ，由T32 T54 O 。先給出如下模擬圖 6.41 ，得出BO.T91 T94 S12 S13 COABCO可能路徑為 2

34、 條：OO1:O T52 A T73 T81S23 S24 T2 T32 T54 另外還可以有路徑：圖 6.41目標(biāo)函數(shù)仍可以表示為：為了求出點(diǎn) A、B、C所在圓弧的圓心，我們?cè)O(shè)計(jì)了如下圖形圖 6.42圖 6.42已知圓P1,P2， P4相當(dāng)于點(diǎn) A在所求的圓弧上，所求的圓弧的圓心為 P3。連接P1P2 ，過點(diǎn)P4作P4P5垂直于 P1P2，取P4P3 r10。1、用兩點(diǎn)式求 P1P2的方程 l1 ：y kx b1 ，結(jié)合 P4 求 p4p5 的方程 l 2 ：2、1xk設(shè)b2；P3、 P4的坐標(biāo)為 x0 ,y0 、x1,y1 ，用 P3、P4的距離建立方程，與 l2 組成方程組：2(x0 x

35、1)(y01y1x1ky1)2b2;10;從而解出所求的圓心 P3 x0, y0 。把相關(guān)的數(shù)據(jù)代入求得過A、B、C的圓的圓心坐標(biāo)如下：ABC(296.8384 ，299.9327)(101.7541 ， 697.3689)(703 ，641)(297.1029 ，298.7325)用 lingo 算法得路徑OO1:OS23T52S24AT2T73T32T81T54BOT91T94S12S13CC 的長 LOO12726.7341OO2: OT54AT73T81BT91T94S12S13C的長 LOO22744.066S23S24T2T32T54O.OO3: OT52AT73T81BT83T13S21S24S2323 的長 LOO32952.1182CS23S24T2T32T54O.OO