機(jī)器人避障問題

機(jī)器人避障問題摘要本文研究了在一個(gè)平面場(chǎng)景里，機(jī)器人通過直線和圓弧轉(zhuǎn)彎，繞過障礙物，到達(dá)目標(biāo)點(diǎn)的問題，解決了到達(dá)目標(biāo)點(diǎn)路徑最短，以及到達(dá)A點(diǎn)時(shí)間最短的問題。文章將路徑劃分為假設(shè)干個(gè)這種線圓結(jié)構(gòu)來求解。對(duì)于途中經(jīng)過節(jié)點(diǎn)的再到達(dá)目標(biāo)點(diǎn)的狀況，我們采用了在拐點(diǎn)和節(jié)點(diǎn)最小轉(zhuǎn)彎半徑的形式.問題一，將其分解成圓線結(jié)構(gòu)進(jìn)行求解，利用枚舉法將最短路徑表示出來，結(jié)果是：O→O→O→A→B→C→O關(guān)鍵詞：最短路徑最短時(shí)間幾何應(yīng)用最優(yōu)化問題一問題重述機(jī)器人從指定點(diǎn)到達(dá)的目標(biāo)點(diǎn)，其行走路徑由直線段和圓弧組成。機(jī)器人轉(zhuǎn)彎路徑由與直線路徑相切的一段圓弧組成，也可以由兩個(gè)或多個(gè)相切的圓弧路徑組成，但每個(gè)圓弧的半徑最小為10個(gè)單位。同時(shí)要求機(jī)器人行走線路與障礙物間的最近距離為10個(gè)單位，機(jī)器人直線行走的最大速度為個(gè)單位/秒。機(jī)器人轉(zhuǎn)彎時(shí)，最大轉(zhuǎn)彎速度為，其中是轉(zhuǎn)彎半徑。要求建立機(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ā)，O→A、O→B、O→C和O→A→B→C→O的最短路徑。(2)機(jī)器人從O(0,0)出發(fā)，到達(dá)A的最短時(shí)間路徑。并要求給出路徑中每段直線段或圓弧的起點(diǎn)和終點(diǎn)坐標(biāo)、圓弧的圓心坐標(biāo)以及機(jī)器人行走的總距離和總時(shí)間。二、問題分析1.問題一要求從起始點(diǎn)o繞過障礙物到達(dá)目標(biāo)點(diǎn)，并且與障礙物最小距離為10，拐彎半徑越小，走的路線越接近直線，在拐彎處畫一個(gè)以拐點(diǎn)為圓心，半徑為10的圓，這樣我們就可以從起始點(diǎn)出發(fā)，分別做圓的切線，直到終點(diǎn)。對(duì)于經(jīng)過路徑中的目標(biāo)點(diǎn)的問題，我們采用最小轉(zhuǎn)彎模式，建立優(yōu)化模型，最終求的最短路徑。2．問題二要求從起始點(diǎn)到達(dá)A點(diǎn)所用的時(shí)間最短，從題意以及生活經(jīng)驗(yàn)可得，拐彎半徑越大，所用時(shí)間越短，拐彎半徑越小，所用時(shí)間越大。半徑最小不低于10，取最大值時(shí)機(jī)器人應(yīng)剛好未碰到4、6三角形，可通過幾何解法計(jì)算出來，并對(duì)時(shí)間進(jìn)行優(yōu)化處理。三、模型假設(shè)假設(shè)機(jī)器人可以抽象成點(diǎn)來處理假設(shè)機(jī)器人的能源充足，且在整個(gè)行走過程中無故障發(fā)生四,符號(hào)說明符號(hào)符號(hào)說明L路徑的總長度第段切線的長度第段圓弧的長度轉(zhuǎn)彎半徑障礙物上的任意點(diǎn)與行走路徑之間的最短距離五模型建立與求解準(zhǔn)備知識(shí)1：具有圓形限定區(qū)域的最短路徑是由兩局部組成的：一局部是平面上的自然最短路徑〔即直線段〕，另一局部是限定區(qū)域的局部邊界，這兩局部是相切的，互相連接的?！?】準(zhǔn)備知識(shí)2:如果一個(gè)圓環(huán)可以繞著環(huán)上一個(gè)定點(diǎn)轉(zhuǎn)動(dòng)，那么過圓環(huán)外兩定點(diǎn)連接一根繩子，并以該圓環(huán)為支撐拉緊繩子，到達(dá)平衡狀態(tài)時(shí)，圓心與該頂點(diǎn)以及兩條切線的延長線的交點(diǎn)共線。也就是說，從起始點(diǎn)到的最短路徑應(yīng)該是假設(shè)干直線段和圓弧，由經(jīng)驗(yàn)知，兩點(diǎn)之間線段最短，那么機(jī)器人走的直線越多，路徑越短，即繞過障礙物的時(shí)候，圓弧半徑越小，行走路徑越短。結(jié)合準(zhǔn)備知識(shí)，轉(zhuǎn)彎半徑按照最小半徑10來計(jì)算。1也是最簡(jiǎn)單的一種情況，從起始點(diǎn)到目標(biāo)點(diǎn)經(jīng)過一個(gè)拐彎。如圖1.有兩種走法，一種是從出發(fā)繞過5的左上角〔80,210〕到達(dá)。一種是從出發(fā)繞過5的右下角〔230,60〕到達(dá)。圖1圖2以從上面繞為例。很顯然，點(diǎn)坐標(biāo)，與垂直，與垂直。如上圖2，設(shè)〔QUOTE為起點(diǎn)，〔QUOTE為目標(biāo)點(diǎn)，圓心為圓的半徑為10，的長度為a，的長度為b，的長度為c，分別為機(jī)器人經(jīng)過拐點(diǎn)時(shí)隔離危險(xiǎn)線拐角小圓弧的切點(diǎn)，角度=QUOTE,=QUOTE,=QUOTE，.設(shè)這段路程機(jī)器人的總路程為L.解法如下：如上圖可得有以下關(guān)系：：在：在中：所以：從而可得：結(jié)果如下：機(jī)器人行走路線=弧=224.7221；b=c=從上面繞到到目標(biāo)點(diǎn)的距離為471.0372。此具體過程如圖可得同理，從下面繞到到目標(biāo)點(diǎn)可以從圖中了解比擬可得，從上面繞到到目標(biāo)點(diǎn)的距離最短，最短路徑為。2.1〕有多條路徑，可以從到正方形5的右下角直接到8的右上角，再繞到。2〕可以從到正方形5的右下角直接到8的右下角，再繞到8的左下角，繞到。3〕可以從到正方形5的左下角到左上角繞過6的右下角再8的右上角繞到。4〕可以從到正方形5的左下角到左上角繞過6的右下角再8的右下角，再繞到8的左下角，繞到。5〕可以從到三角形6的左下角再到三角形6的上頂點(diǎn)，繞到矩形7右下角，到矩形7右上角，再到8的右上角，繞到。6〕可以從到三角形6的左下角再到三角形6的上頂點(diǎn)，繞到矩形7右下角，到矩形7右上角，再到8的右下角，到8的左下角，繞到。在這些可能的路徑中，有以下兩種新的情形：第一、從起始點(diǎn)到目標(biāo)點(diǎn)經(jīng)過同側(cè)拐彎〔即圓弧在所走路徑的同側(cè)〕。圖2運(yùn)用幾何知識(shí)，可以看出，連接，，，，可以得到與1相同的兩段路程，解決過程跟上述相同。第二、從起始點(diǎn)到目標(biāo)點(diǎn)經(jīng)過異側(cè)拐彎〔即圓弧在所走路徑的異側(cè)〕。我們假設(shè)兩圓心坐標(biāo)分別為和,半徑均為r，M點(diǎn)坐標(biāo)為，那么我們很容易可以求得:這樣我們就可以利用1〕中的方法，先求A到M，再求M到B，這樣分兩段就可以求解。同理如果有更多的轉(zhuǎn)彎，我們同樣可以按照此種方法分解。結(jié)果如下：第三、這種情況增加了一個(gè)圓形，在繞過圓形的時(shí)候，以圓形的圓心為圓心，以圓形半徑加10為半徑，畫弧。與上面兩種情況類似，只不過增加了路徑的長度和情況。經(jīng)上面分析發(fā)現(xiàn)，機(jī)器人行走路線與出發(fā)點(diǎn)到目標(biāo)點(diǎn)連線段越近，越趨于最短路。于是，我們著重看了以下幾種情況：1〕可以從到三角形4右下角繞到矩形12左上角，到正方形11右下角，到11右上角，再繞到。2〕可以從到正方形5左上角繞到3左上角，到圓2，到11右下角，到11右上角，再繞到。計(jì)算結(jié)果：第四、在這個(gè)過程中，機(jī)器人不僅要繞過障礙物，還要經(jīng)過三點(diǎn)，以點(diǎn)為例，在經(jīng)過點(diǎn)時(shí)，機(jī)器人要轉(zhuǎn)彎，轉(zhuǎn)彎圓弧要經(jīng)過，圓弧以10為半徑。在此，要用到準(zhǔn)備知識(shí)2.二、最短時(shí)間路徑根據(jù)題目和經(jīng)驗(yàn)判斷，拐彎越大速度越快，也就是拐彎時(shí)半徑可以增大，但不能碰到上面的三角形。所以對(duì)半徑有范圍要求。最大半徑確實(shí)定：圖，由此能夠計(jì)算出的直線方程，的縱坐標(biāo)為290，由直線方程可以判斷出的橫坐標(biāo)。〔241,290〕那么所在的直線方程就可以確定，為六、模型推廣6.1問題深入分析在此題中只有12個(gè)障礙物，按照線圓結(jié)構(gòu)畫出從起點(diǎn)到達(dá)目標(biāo)點(diǎn)的路徑是有限的，我們完全可以采用窮舉法把這些路徑列出來，然后比擬大小取最小者即可，但是我們可以設(shè)想如果這個(gè)區(qū)域內(nèi)有n個(gè)障礙物，那么按照線圓結(jié)構(gòu)從起點(diǎn)到達(dá)目標(biāo)點(diǎn)的可能路徑就有無數(shù)多條，這時(shí)我們?nèi)绻诓捎酶F舉法是不現(xiàn)實(shí)的。所以我們必須尋求新的算法來解決這個(gè)問題。由上述分析我們有了這樣一個(gè)想法：先求出所有的切線，包括出發(fā)點(diǎn)和目標(biāo)點(diǎn)到所有圓弧的切線以及所有圓弧與圓弧之間的切線，然后把這且曲線看成是圖中的，給這些定點(diǎn)賦一個(gè)等于切線長度的權(quán)值，如果某兩條切線有一個(gè)公切圓弧，那么代表這兩條曲線的頂點(diǎn)是一條直線的兩個(gè)端點(diǎn)，邊上的權(quán)值等于這兩條切線之間的劣弧長度。然后在這張圖中加一個(gè)源點(diǎn)和終點(diǎn)，那么在所有代表出發(fā)點(diǎn)與其它圓弧之間切線的頂點(diǎn)與源點(diǎn)連成一條邊，權(quán)值均為0，同理在所有代表目標(biāo)點(diǎn)到其它圓弧切線的頂點(diǎn)與終點(diǎn)連成一條邊，權(quán)值均為0，這樣題目就轉(zhuǎn)化成了求源點(diǎn)到達(dá)終點(diǎn)之間的最短路徑問題了，這里最短路徑就是指經(jīng)過所有頂點(diǎn)與邊的權(quán)值之和最小。我們可以采用Dijkstra算法進(jìn)行求解。6.2模型的進(jìn)一步求解根據(jù)分析，在有假設(shè)干個(gè)障礙物的區(qū)域中，我們把按照線圓結(jié)構(gòu)畫出從出發(fā)點(diǎn)到目標(biāo)點(diǎn)的路徑圖依據(jù)想法轉(zhuǎn)換成了下面這張圖，圖中的A和G點(diǎn)就是添加的源點(diǎn)和終點(diǎn)，其它節(jié)點(diǎn)均是出發(fā)點(diǎn)和目標(biāo)點(diǎn)到圓弧的切線和圓弧與圓弧之間的切線轉(zhuǎn)化而成。對(duì)于最短路徑的求解，有以下步驟：1〕我們畫出出發(fā)點(diǎn)和目標(biāo)點(diǎn)和各個(gè)圓弧的切線，以及圓弧與圓弧之間的切線，但是切線有可能經(jīng)過障礙物的內(nèi)部或危險(xiǎn)區(qū)域，也可能出現(xiàn)切線重復(fù)的狀況，既有很多不合法的切線。于是我們對(duì)模型做了以下修正：檢驗(yàn)切線兩個(gè)端點(diǎn)是否在障礙物內(nèi)部。檢驗(yàn)切線是否障礙物的對(duì)角線相交。檢驗(yàn)圓弧所對(duì)應(yīng)的圓心，即障礙物的頂點(diǎn)到切線的距離是否小于1。如果以上三種情況滿足其一，我們規(guī)定對(duì)應(yīng)這段切線的頂點(diǎn)為M〔M為無窮大〕。另外還有如下列圖所示的一種特殊情況：兩個(gè)大小相同在同一水平或者豎直位置上，不考慮切線滿足1、2、3的狀況它們由2條內(nèi)公切線，8條外公切線，但是有6條外公切線是重復(fù)的。因此我們作如下規(guī)定:如果某條切線與某段圓弧相切，且切點(diǎn)不在切線的端點(diǎn)上，那么該切線為不合法。權(quán)值矩陣中表示它的頂點(diǎn)也為M。轉(zhuǎn)化成如7.21所示的形式。假設(shè)轉(zhuǎn)化過后有m條合法切線，那么就有m個(gè)頂點(diǎn)，設(shè)這些點(diǎn)的權(quán)值〔〕，即第條合法曲線的長度。為邊的權(quán)值，即第條弧的長度。3〕然后把路徑圖轉(zhuǎn)化成如圖6.21所示，按照求得權(quán)值矩陣給圖中的頂點(diǎn)及邊長賦值。4〕最后依據(jù)Dijkstra算法求得最短路徑。 七、模型評(píng)價(jià)一、模型優(yōu)點(diǎn)1、運(yùn)用多個(gè)方案對(duì)路徑進(jìn)行優(yōu)化，在相對(duì)優(yōu)化之中取得最優(yōu)解。模型優(yōu)化后用解析幾何進(jìn)行求解，精確度較高。

3、模型簡(jiǎn)單易懂，便于實(shí)際檢驗(yàn)及應(yīng)用。4、運(yùn)用圖像更直觀方便運(yùn)算。5、運(yùn)用多個(gè)方案對(duì)路徑進(jìn)行選擇，在相對(duì)短的路徑中得到最短路徑二、模型缺陷1、此模型適于全局規(guī)劃，獲得精確解卻失去了效率。

2、在障礙物較多時(shí)，且形狀不規(guī)那么時(shí)，模型需要進(jìn)一步改良。參考文獻(xiàn)[1]譚永基，數(shù)學(xué)模型，上海，復(fù)旦大學(xué)出版社，2011[2]邦迪，圖論及其應(yīng)用，西安，西安科學(xué)出版社19844]尤承業(yè)，解析幾何，北京，北京大學(xué)出版社，2004[5]周培德，計(jì)算幾何—算法與設(shè)計(jì)，北京清華大學(xué)出版社，2005[6]曹戈，《MATLAB教程及實(shí)訓(xùn)》，機(jī)械工業(yè)出版社2008年5月1]洪毅林健良陶志穗，《數(shù)學(xué)模型》，北京：高等教育出版社，2004年[7]袁新生邵大宏郁時(shí)煉，《LINGO和Excel在數(shù)學(xué)建模中的應(yīng)用》，北京：科學(xué)出版社，2007年尤承業(yè)，解析幾何，北京，北京大學(xué)出版社，2004附錄1>>x1=0;>>y1=0;>>x2=80;>>y2=210;>>x3=300;>>y3=300;>>a=((x2-x1)^2+(y2-y1)^2)^(1/2);>>b=((x3-x2)^2+(y3-y2)^2)^(1/2);>>c=((x1-x3)^2+(y1-y3)^2)^(1/2);>>a,b,ca=b=c=>>q=acos((b^2+a^2-c^2)/(2*b*a))q=>>R=10;>>r=acos(R/a);>>s=acos(R/b);>>t=2*pi-q-r-s;>>L=(b^2-R^2)^(1/2)+(a^2-R^2)^(1/2)+R*tL=附錄2>>%1為O(0，0)2為C(60，300)3為D(150，435)4為E(220，470)5為F(220，530)%6為G(150，600)7為B(100，700)OC=aCD=bKD=cX1=0;Y1=0;X2=60;Y2=300;X3=150;Y3=435;X4=220;Y4=470;X5=220;Y5=530;X6=150;Y6=600;X7=100;Y7=700;a=((X2-X1)^2+(Y2-Y1)^2)^(1/2);%CO的長b=((X3-X2)^2+(Y3-Y2)^2)^(1/2)%CD的長c=((X3-X1)^2+(Y3-Y1)^2)^(1/2);%OD的長r=10;h=acos(r/a);q=acos((a^2+b^2-c^2)/(2*a*b));%OCD度數(shù)t1=2*pi-q-h-1/2*pid=[((X4-X3)^2+(Y4-Y3)^2)^(1/2)]/2;%DE/2的長u=(d^2-r^2)^(1/2);%MH(MP/2)的長J=2*ui=acos(r/d);e=((X4-X2)^2+(Y4-Y2)^2)^(1/2);%CE的長p=acos(((2*d)^2+b^2-e^2)/(2*(2*d)*b));t2=2*pi-p-i-1/2*pif=((X5-X3)^2+(Y5-Y3)^2)^(1/2);%FD的長g=((X5-X4)^2+(Y5-Y4)^2)^(1/2)%FE的長z=acos(((2*d)^2+g^2-f^2)/(2*d*2*g));h=acos(r/d);t3=2*pi-z-h-1/2*pii=((X5-X6)^2+(Y6-Y5)^2)^(1/2);%GF的長j=((X4-X6)^2+(Y6-Y4)^2)^(1/2);%GE的長y=acos(r/i);x=acos((i^2+g^2-j^2)/(2*i*g));t4=2*pi-y-x-1/2*pil=2*((i/2)^2-r^2)^(1/2)%SW的長k=((X6-X7)^2+(Y7-Y6)^2)^(1/2);%BG的長m=((X5-X7)^2+(Y7-Y5)^2)^(1/2);%BF的長w=acos((k^2+i^2-m^2)/(2*m*i));u=acos(r/k);v=acos(r/i);t5=2*pi-w-u-vI=(k^2-r^2)^(1/2)%TW的長H=(a^2-r^2)^(1/2)L=H+t1+b+t2+J+t3+g+t4+l+t5+Ib=t1=J=t2=g=60t3=t4=l=t5=I=H=L=附錄3>>%1為O(0，0)2為D3為E4為F5為G6為H7為CX1=0;Y1=0;X2=180;Y2=210;X3=400;Y3=330;X4=550;Y4=450;X5=720;Y5=520;X6=720;Y6=600;X7=700;Y7=640;r=10;r2=80;a=((X2-X1)^2+(Y2-Y1)^2)^(1/2);%OD的長b=((X3-X2)^2+(Y3-Y2)^2)^(1/2)%DE的長c=((X3-X1)^2+(Y3-Y1)^2)^(1/2);%OE的長q=acos((a^2+b^2-c^2)/(2*b*a));d=(a^2-r^2)^(1/2)%OI的長q1=acos(r/a);t1=2*pi-q-q1-1/2*pi%第一個(gè)弧長e=((X4-X2)^2+(Y4-Y2)^2)^(1/2);%DF的長f=((X4-X3)^2+(Y4