機器人避障問題

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

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

2、在障礙物較多時，且形狀不規(guī)那么時，模型需要進一步改良。參考文獻[1]譚永基，數(shù)學模型，上海，復旦大學出版社，2011[2]邦迪，圖論及其應用，西安，西安科學出版社19844]尤承業(yè)，解析幾何，北京，北京大學出版社，2004[5]周培德，計算幾何—算法與設計，北京清華大學出版社，2005[6]曹戈，《MATLAB教程及實訓》，機械工業(yè)出版社2008年5月1]洪毅林健良陶志穗，《數(shù)學模型》，北京：高等教育出版社，2004年[7]袁新生邵大宏郁時煉，《LINGO和Excel在數(shù)學建模中的應用》，北京：科學出版社，2007年尤承業(yè)，解析幾何，北京，北京大學出版社，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%第一個弧長e=((X4-X2)^2+(Y4-Y2)^2)^(1/2);%DF的長f=((X4-X3)^2+(Y4