行走機器人避障問題數(shù)學建模.doc

1、機器人行走問題摘要 本文研究了機器人避障最短路徑的問題。主要研究了在一個區(qū)域中存在四個障礙物，由出發(fā)點到達目標點以及由出發(fā)點經(jīng)過途中的若干目標點到達最終目標點的兩種情形。我們通過證明具有圓形限定區(qū)域的最短路徑是由兩部分組成的：一部分是平面上的自然最短路徑（即直線段）,另一部分是限定區(qū)域的部分邊界，這兩部分是相切的，互相連接的。依據(jù)這個結(jié)果，我們可以認為最短路徑一定是由線和圓弧做組成，因此我們建立了線圓結(jié)構(gòu)，這樣無論路徑多么復雜,我們都可以將路徑劃分為若干個這種線圓結(jié)構(gòu)來求解。對于途中經(jīng)過節(jié)點的再到達目標點的狀況,我們采用了兩種方案，一種是在拐點和節(jié)點都采用最小轉(zhuǎn)彎半徑的形式，另一種是適當擴大拐

2、點處的轉(zhuǎn)彎半徑，使得機器人能夠沿直線通過途中的目標點。然后建立了最優(yōu)化模型對兩種方案分別進行求解。問題一，我們很容易分解成線圓結(jié)構(gòu)來求解，然后把可能路徑的最短路徑采用窮舉法列舉出來，最終得出最短路徑：R A 最短路徑為:70。5076R B 最短路徑為：107.9587R C 最短路徑為：102。0514問題二，我們方案都進行優(yōu)化，求得最終結(jié)果：第一種方案最短路徑為：156.471第二種方案最短路徑為：157.752關(guān)鍵詞 最短路徑 最優(yōu)化模型 避障路徑 解析幾何一 、問題重述下圖是一個100×80的平面場景圖，在R(0,0)點處有一個機器人，機器人只能在該100×80的范

3、圍內(nèi)活動,圖中四個矩形區(qū)域是機器人不能與之發(fā)生碰撞的障礙物，障礙物的數(shù)學描述分別為B1（20，40；5，10）、B2（30，30；10，15）、B3（70，50;15，5）、B4（85，15；5,10），其中B1（20，40；5,10）表示一個矩形障礙物,其中心坐標為（20，40），5表示從中心沿橫軸方向左右各5個單位，即矩形沿橫軸方向長5×2=10個單位，10表示從中心沿縱軸方向上下各10個單位，即矩形沿縱軸方向長10×2=20個單位,所以，障礙物B1的中心在（20，40），大小為10×20個單位的矩形，其它三個障礙物的描述完全類似。在平面場景中、障礙物外指定一

4、點為機器人要到達的目標點（要求目標點與障礙物的距離至少超過1個單位)，為此，須要確定機器人的最優(yōu)行走路線由直線段和圓弧線段組成的光滑曲線，其中圓弧線段是機器人轉(zhuǎn)彎路線，機器人不能折線轉(zhuǎn)彎,轉(zhuǎn)彎路徑是與直線相切的一圓形曲線段，也可以是兩個或多個相切的圓弧曲線段組成，但每個圓形路線的半徑都必須大于某個最小轉(zhuǎn)彎半徑,假設(shè)為1個單位.另外，為了不與障礙物發(fā)生碰撞,要求機器人行走線路與障礙物間的最短距離為1個單位，越遠越安全,否則將發(fā)生碰撞，若碰撞發(fā)生，則機器人無法到達目標點，行走失敗。請回答如下問題:1. 場景圖中有三個目標點A（50，40）、B（75，60）、C(95,20），請用數(shù)學建模的方法給出

5、機器人從R（0,0）出發(fā)安全到達每個目標點的最短路線.2. 求機器人從R（0,0）出發(fā)，依次安全通過A、B到達C的最短路線。二、 問題分析1、問題一中要求求定點R（0, 0）按照一定的行走規(guī)則繞過障礙物到達目標點的最短路徑，我們先可以包絡(luò)線畫出機器人行走的危險區(qū)域，這樣的話，拐角處就是一個半徑為1的四分之一圓弧,通過那么然后采用拉繩子的方法尋找可能的最短路徑（比如求R和A之間的最短路徑,我們就可以連接R和A之間的一段繩子，以拐角處的圓弧為支撐拉緊，那么這段繩子的長度便是R到A的一條可能的最短路徑)，然后采用窮舉法列出R到每個目標點的可能路徑的最短路徑，然后比較其大小便可得出R到目標點的最短路徑

6、。2、問題二中要求求定點R（0， 0）經(jīng)過中間的若干點按照一定的規(guī)則繞過障礙物到達目標點，這使我們考慮就不僅僅是經(jīng)過障礙物拐點的問題，也應該考慮經(jīng)過路徑中的目標點處轉(zhuǎn)彎的問題，這時簡單的線圓結(jié)構(gòu)就不能解決這種問題，我們在拐點及途中目標點處都采用最小轉(zhuǎn)彎半徑的形式，也可以適當?shù)淖儞Q拐點處的拐彎半徑，使機器人能夠沿直線通過途中的目標點，然后建立優(yōu)化模型對這兩種方案分別進行優(yōu)化，最終求得最短路徑。三、 模型假設(shè)1、假設(shè)障礙物全是矩形。2、假設(shè)機器人能夠抽象成點來處理。四、符號說明符號符號說明L路徑的總長度第段切線的長度第段圓弧的長度轉(zhuǎn)彎半徑障礙物上的任意點與行走路徑之間的最短距離五、模型的建立5.1

7、 先來證明一個猜想：猜想一：具有圓形限定區(qū)域的最短路徑是由兩部分組成的:一部分是平面上的自然最短路徑（即直線段),另一部分是限定區(qū)域的部分邊界，這兩部分是相切的,互相連接的。（即問題分析中的拉繩子拉到最緊時的狀況)證明：假設(shè)在平面中有A（a，0)和B(-a,0）兩點,中間有一個半圓形的障礙物，證明從A到B的最路徑為AEFB。 平面上連接兩點最短的路徑是通過這兩點的直線段，但是連接兩點的線段于障礙物相交，所以設(shè)法嘗試折線路徑.在y軸上取一點C(0，y）,若y適當大，則折線ACB與障礙物不相交,折線ACB的長度為： 顯然隨著y的減小而減小,減小得，即,使得與與障礙物相切,切點分別為E和F,顯然是這

8、種折線路徑中最短的.由于滿足0<<的角滿足tan，所以易知弧度EF小于的長， 即EF<EC1F，從而EF+FB，記線段AE、弧度EF、線段FB為AEFB，那么AEFB比任何折線路徑都短.下面在考察一條不穿過障礙物的任何一條路徑，設(shè)其分別于OE和OF的延長線交與P、Q兩點，記A和P之間的路徑長度為AP，顯然AP>AP|，又由AEEO，所以AP>AE,從而AP>AE，同理可得BQBF.再來比較PQ之間路徑長度PQ和圓弧EF的長度的大小。若PQ之間的路徑可有極坐標方程=()，則有>1,可得：PQ=2+2dd-EF 亦即路徑APQB的長度超過路徑AEFB的長度

9、.以上證明足以說明了AEFB是滿足條件A到B的最短路徑.5。2 模型準備一 有了4。1中的定理，我們就可以這樣認為,起點到目標點無論中間障礙物有多少，最短路徑都應該是若干個線圓結(jié)構(gòu)所組成。在本題中存在障礙物的狀況,且障礙物在拐點處的危險區(qū)域是一個半徑為1的圓弧，所以結(jié)合定理一，我們易知，求兩點之間的最短路徑中的轉(zhuǎn)彎半徑我們應該按照最小的轉(zhuǎn)彎半徑來算才能達到最優(yōu)。線圓結(jié)構(gòu)5.21 1）如上圖,設(shè)A（x1，y1）為起點,B（x2，y2）為目標點，C（x3，y3）和D（x4，y4）分別為機器人經(jīng)過拐點分別于隔離危險線拐角小圓弧的切點，圓心為O(x5，y5）,圓的半徑為r，AB的長度為a，AO的長度為

10、b，BO的長度為c，角度=, =， =,。求ACDB的長度，設(shè)為L.解法如下:如上圖可得有以下關(guān)系：在:在中：所以：從而可得：2)而對于下圖兩種情況我們不能直接采用線圓的結(jié)構(gòu)來解決，需要做簡單的變換. 情況一：線圓結(jié)構(gòu)5。22 我們假設(shè)兩圓心坐標分別為和，半徑均為r，M點坐標為，那么我們很容易可以求得:這樣我們就可以利用1)中的方法，先求A到M，再求M到B,這樣分兩段就可以求解。同理如果有更多的轉(zhuǎn)彎,我們同樣可以按照此種方法分解。情況二：線圓結(jié)構(gòu)5。23 這里我們依然設(shè)圓心坐標分別為和，半徑均為r,這樣我們可以得到：那么直線方程為：因為公切線DE與平行，那么DE的直線方程可以表示為：其中：那么

11、把公切線的方程于圓的方程聯(lián)立,渴可以求得切點D和E的坐標.這樣用D和E任意一點作為分割點都可以將上圖分割成兩個4.21所示的線圓結(jié)構(gòu)，這樣就可以對其進行求解.同理多個這樣的轉(zhuǎn)彎時，用同樣的方法可以進行分割。5。3 模型準備二 一、對于從起點經(jīng)過若干點然后再到達目標點的狀況，因為不能走折線路徑,我們就必須考慮在經(jīng)過路徑中的一個目標點時轉(zhuǎn)彎的狀況。為了研究這個問題的方便，我們先來證明一個猜想:猜想二：如果一個圓環(huán)可以繞著環(huán)上一個定點轉(zhuǎn)動，那么過圓環(huán)外兩定點連接一根繩子，并以該圓環(huán)為支撐拉緊繩子，達到平衡狀態(tài)時，圓心與該頂點以及兩條切線的延長線的交點共線. 圖5.31證明猜想：如圖4。31所示，E點

12、就是圓環(huán)上的一個頂點，ACDB就是拉緊的繩子，就是切線AC和BD的延長線的交點，證明、E、三點共線。我們可以用力學的知識進行證明，因為是拉緊的繩子，所以兩邊的繩子拉力相等，設(shè)為，它們的合力設(shè)為,定點對圓環(huán)的作用力設(shè)為。那么由幾何學的知識我們可以知道一定與共線，而又由力的平衡條件可知:=-即與共線.綜上所述、和三點一定共線。二、有了以上這個定理我們可以建立以下模型：如圖4.32，要求求出機器人從A繞過障礙物經(jīng)過M點到達目標點B的最短路徑，我們采用以下方法：用一根釘子使一個圓環(huán)定在M點，使這個圓環(huán)能夠繞M點轉(zhuǎn)動。然后連接A和B的繩子并以這些轉(zhuǎn)彎處的圓弧為支撐（這里轉(zhuǎn)彎處圓弧的半徑均按照最小轉(zhuǎn)彎半徑

13、來計算），拉緊繩子，那么繩子的長度就是A到B的最短距離。我們可以把路徑圖抽象為以下的幾何圖形。下面我們對這段路徑求解：圖5。32如圖，A是起點，B是終點,和是兩個固定的圓，是一個可以繞M（p,q)點轉(zhuǎn)動的圓環(huán)，三個圓的半徑均為r，C、D、E、F、G、H均為切點。a、b、c、e，f分別是A、A、A、的長度。A、B、均是已知點，是未知點。那么最短路徑就可以表示為：L=|AC|+CD+DE+EF+|FG|+GH+HB|因為點的坐標未知，所以我們就不能用模型一中的線圓結(jié)構(gòu)對其進行求解.故得先求出點的坐標.設(shè)坐標為（m,n），、分別為（=1、2、3、4、5)，、分別為、。這樣便有以下關(guān)系：在中:在中：在

14、中：在中:則：又因為一定會在的角平分線上，所以滿足：我們采用向量的形式來求，易知的一個方向向量：而與垂直，故其一個方向向量：而：所以：綜合以上式子可以求得的坐標,從而可以得出路徑的長為：=GH+HB,這可以采用模型一中的線圓結(jié)構(gòu)來求解。5.4 模型準備三求解從起點經(jīng)過若干個點再到達目標點的問題,與4.4不同,我們還可以有另一種方案，即適當擴大障礙物拐點處的拐彎半徑使機器人能夠沿直線通過路徑中的目標點。這樣拐點處拐彎圓弧的半徑和圓心都是個變量，對于該題，那么我們可以首先設(shè)定三個圓心、,然后按照以下步驟進行作圖：1） 給定，以為圓心，為半徑，畫圓，然后過R點做圓的切線,切點為D.然后過A點做的切線

15、設(shè)為，切點為E.2） 然后做F垂直于，垂足為F，F(xiàn)的長就是，然后以為圓心，為半徑畫圓。很顯然能由來確定，即.3） 然后過B點做的切線為，切點為G,再過F垂直于，垂足為H，那么H的長度就是，然后以為圓心，為半徑,畫圓。很顯然能由來確定，即=。4） 過C做的切線。這就完成了由R經(jīng)過A和B在到達C的路徑.5） 然后再變換、，可得到新的路徑。找出最小者即可.5。5 模型的建立 假設(shè)機器人從起點R到到目標點，由4.2知路徑一定是由圓弧和線段組成,設(shè)有m條線段，n條圓弧。那么目標函數(shù)可以表示為:用此模型就可以對起點到目標點之間的路徑進行優(yōu)化求解.六、模型的求解6。1 問題一一、以下給出了R到個目標點的可能

16、路徑的最短路徑:1）如圖一,解決的就是R到目標點A的最短路徑問題，很顯然的一個問題是機器人從B1的上方走的最短路徑肯定是大于機器人從B2下方走的最短路徑,所以機器人從B1方向走的最優(yōu)路線我們在圖一中沒有給出。圖一中，藍線就可以為機器人隔出了危險區(qū)域，紅線表示的就是通過B2點的圓為支撐而拉緊的繩子，這樣計算出繩子的長度就是R到A的最短路徑.2）如圖二，解決的是R到目標點B的最短路徑問題,圖中給出了可能的三條路徑的最短路徑（圖中的紅線所示），我們可以分別計算出三條可能路徑的最短路徑的長度,然后進行比較，取最小者就是R到目標點B的最優(yōu)路徑。3）如圖三，解決的是R到目標點C的最短路徑問題。圖中給出了兩

17、條可能路徑的最短路徑（圖中的紅線所示），我們同樣可以分別計算出兩條可能的最短路徑，取最小者就是R到目標點C的最優(yōu)路徑。圖5.11圖6。12圖6.13二、 然后用matlab求解結(jié)果如下:1)R到目標點A的最短距離為70。50762)R到目標點B的可能路徑有三條,即就有三條可能的最短路徑。 如圖二，機器人走最上面這條路徑到達B,直接用matlab求得最短路徑為114。1611 而機器人經(jīng)過中間一條路徑到達B，這條路徑由三條線段和兩段圓弧組成，直接用三中的解法是結(jié)不出來的。于是我們做了如下變換,先求出中間一條直線的中點設(shè)為F，那么可以采用三中的解法，分別求R到F和F到B的最短路徑，然后兩段相加,便

18、可求出R到B的最短路徑.求解結(jié)果為107。9587 機器人經(jīng)過最下邊一條路徑，同理這條路徑由四條直線和三個圓弧組成，同樣可以采取中的變換,分三部分求解。求解結(jié)果如下為116。1256 綜合所述,R到目標點B的最短路徑為107。95873)R到目標點C的可能路徑由兩條,和2中的方法一樣，最終求解結(jié)果R到 目標點C的最短路徑為102.05146。2 問題二 一、我們先按照4。3中的思想來進行求解，這樣我們可以做出5。21所示的示意圖： 圖6.21采用模型準備二中的計算方法，用lingo求解的結(jié)果最短路徑為：156。471二、我們在采用模型準備三中的算法進行求解,可以畫出5。22所示的示意圖：圖5.

19、22采用模型進行優(yōu)化，運用lingo求解的最短路徑為：157.752顯然運用模型準備二中的方法求解的結(jié)果小于用模型準備三中的方法求解的結(jié)果，說明模型準備二的方法優(yōu)于模型準備三的方法。七、模型推廣7.1 問題深入分析 在本題中只有四個障礙物,按照線圓結(jié)構(gòu)畫出從起點到達目標點的路徑是有限的，我們完全可以采用窮舉法把這些路徑列出來，然后比較大小取最小者即可，但是我們可以設(shè)想如果這個區(qū)域內(nèi)有n個障礙物，那么按照線圓結(jié)構(gòu)從起點到達目標點的可能路徑就有無數(shù)多條，這時我們?nèi)绻诓捎酶F舉法是不現(xiàn)實的。所以我們必須尋求新的算法來解決這個問題。由上述分析我們有了這樣一個想法：先求出所有的切線，包括出發(fā)點和目標點到

20、所有圓弧的切線以及所有圓弧與圓弧之間的切線,然后把這且曲線看成是圖6.11中的，給這些定點賦一個等于切線長度的權(quán)值，如果某兩條切線有一個公切圓弧，則代表這兩條曲線的頂點是一條直線的兩個端點，邊上的權(quán)值等于這兩條切線之間的劣弧長度.然后在這張圖中加一個源點和終點，那么在所有代表出發(fā)點與其它圓弧之間切線的頂點與源點連成一條邊，權(quán)值均為0，同理在所有代表目標點到其它圓弧切線的頂點與終點連成一條邊,權(quán)值均為0，這樣題目就轉(zhuǎn)化成了求源點到達終點之間的最短路徑問題了,這里最短路徑就是指經(jīng)過所有頂點與邊的權(quán)值之和最小.我們可以采用Dijkstra算法進行求解。7.2 模型的進一步求解根據(jù)6。1的分析，在有若

21、干個障礙物的區(qū)域中，我們把按照線圓結(jié)構(gòu)畫出從出發(fā)點到目標點的路徑圖依據(jù)6。1中的想法轉(zhuǎn)換成了下面這張圖,圖中的A和G點就是添加的源點和終點,其它節(jié)點均是出發(fā)點和目標點到圓弧的切線和圓弧與圓弧之間的切線轉(zhuǎn)化而成。圖7。21 對于最短路徑的求解，有以下步驟：1）我們畫出出發(fā)點和目標點和各個圓弧的切線，以及圓弧與圓弧之間的切線,但是切線有可能經(jīng)過障礙物的內(nèi)部或危險區(qū)域,也可能出現(xiàn)切線重復的狀況，既有很多不合法的切線.于是我們對模型做了以下修正：1、 檢驗切線兩個端點是否在障礙物內(nèi)部.2、 檢驗切線是否障礙物的對角線相交。3、 檢驗圓弧所對應的圓心,即障礙物的頂點到切線的距離是否小于1。如果以上三種情

22、況滿足其一，我們規(guī)定對應這段切線的頂點為M（M為無窮大）。4、 另外還有如下圖所示的一種特殊情況：兩個大小相同在同一水平或者豎直位置上,不考慮切線滿足1、2、3的狀況它們由2條內(nèi)公切線，8條外公切線，但是有6條外公切線是重復的。因此我們作如下規(guī)定：如果某條切線與某段圓弧相切，且切點不在切線的端點上，則該切線為不合法。權(quán)值矩陣中表示它的頂點也為M。圖7。222） 然后把合法的切線與這些切線之間的劣弧轉(zhuǎn)化成如7.21所示的形式。假設(shè)轉(zhuǎn)化過后有m條合法切線，那么就有m個頂點,設(shè)這些點的權(quán)值(），即第條合法曲線的長度。為邊的權(quán)值，即第條弧的長度。 3）然后把路徑圖轉(zhuǎn)化成如圖6.21所示，按照求得權(quán)值矩

23、陣給圖中的頂點及邊長賦值. 4）最后依據(jù)Dijkstra算法求得最短路徑。八、模型評價一、模型優(yōu)點 1、運用多個方案對路徑進行優(yōu)化，在相對優(yōu)化之中取得最優(yōu)解。2、模型優(yōu)化后用解析幾何進行求解，精確度較高。3、模型簡單易懂，便于實際檢驗及應用。二、模型缺陷 1、此模型適于全局規(guī)劃，獲得精確解卻失去了效率。2、在障礙物較多時,且形狀不規(guī)則時,模型需要進一步改進。九、參考文獻1譚永基，數(shù)學模型，上海,復旦大學出版社，20112邦迪，圖論及其應用，西安，西安科學出版社 19843胡海星,RPG游戲中精靈的移動問題,雜志程序員 2011；4尤承業(yè)，解析幾何，北京，北京大學出版社，20045周培德，計算幾

24、何算法與設(shè)計，北京清華大學出版社,2005十、附錄1)%求解一次轉(zhuǎn)彎所經(jīng)路線總長T：初始點 V:轉(zhuǎn)彎圓弧圓心 W:到達點 function result=zongchang(T，W，V,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+VW2TW2)/(2*TV*VW）;alpha2=acos(r/TV）；alpha3=acos（r/VW）；alpha4=2*pi-alpha1alpha2-alpha3；alpha4為轉(zhuǎn)彎圓心角TS1=sqrt（TV2-r2）;TS1,TS2均為圓弧切線S2W=sqrt(VW2r2);S1S2hu=r*alpha4；r