行走機器人避障問題

機器人行走問題摘要本文研究了機器人避障最短路徑的問題。主要研究了在一個區(qū)域中存在四個障礙物，由出發(fā)點到達(dá)目標(biāo)點以及由出發(fā)點經(jīng)過途中的若干目標(biāo)點到達(dá)最終目標(biāo)點的兩種情形。我們通過證明具有圓形限定區(qū)域的最短路徑是由兩部分組成的：一部分是平面上的自然最短路徑（即直線段），另一部分是限定區(qū)域的部分邊界，這兩部分是相切的，互相連接的。依據(jù)這個結(jié)果，我們可以認(rèn)為最短路徑一定是由線和圓弧做組成，因此我們建立了線圓結(jié)構(gòu)，這樣無論路徑多么復(fù)雜，我們都可以將路徑劃分為若十個這種線圓結(jié)構(gòu)來求解。對于途中經(jīng)過節(jié)點的再到達(dá)目標(biāo)點的狀況，我們采用了兩種方案，一種是在拐點和節(jié)點都采用最小轉(zhuǎn)彎半徑的形式，另一種是適當(dāng)擴大拐點處的轉(zhuǎn)彎半徑，使得機器人能夠沿直線通過途中的目標(biāo)點。然后建立了最優(yōu)化模型對兩種方案分別進(jìn)行求解。問題一，我們很容易分解成線圓結(jié)構(gòu)來求解，然后把可能路徑的最短路徑采用窮舉法列舉出來，最終得出最短路徑：RTA最短路徑為：70.5076RTB最短路徑為：107.9587RTC最短路徑為：102.0514問題二，我們方案都進(jìn)行優(yōu)化，求得最終結(jié)果：第一種方案最短路徑為：156.471第二種方案最短路徑為：157.752關(guān)鍵詞最短路徑最優(yōu)化模型避障路徑解析幾何一、問題重述下圖是一個100X80的平面場景圖，在R(0,0)點處有一個機器人，機器人只能在該100X80的范圍內(nèi)活動，圖中四個矩形區(qū)域是機器人不能與之發(fā)生碰撞的障礙物，障礙物的數(shù)學(xué)描述分別為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)表示一個矩形障礙物，其中心坐標(biāo)為(20,40),5表示從中心沿橫軸方向左右各5個單位，即矩形沿橫軸方向長5X2=10個單位，10表示從中心沿縱軸方向上下各10個單位，即矩形沿縱軸方向長10X2=20個單位，所以，障礙物B1的中心在(20,40)，大小為10X20個單位的矩形，其它三個障礙物的描述完全類似。1ie11(20/)1ie11(20/))8060(75,6。)B3(70,50;15,5)20B2(30,30;1Q15)204060SO__20B2(30,30;1Q15)204060SO__I..10040A(50,40)64(85,15;5,10)■?i■C(95,啊在平面場景中、障礙物外指定一點為機器人要到達(dá)的目標(biāo)點(要求目標(biāo)點與障礙物的距離至少超過1個單位)，為此，須要確定機器人的最優(yōu)行走路線——由直線段和圓弧線段組成的光滑曲線，其中圓弧線段是機器人轉(zhuǎn)彎路線，機器人不能折線轉(zhuǎn)彎，轉(zhuǎn)彎路徑是與直線相切的一圓形曲線段，也可以是兩個或多個相切的圓弧曲線段組成，但每個圓形路線的半徑都必須大于某個最小轉(zhuǎn)彎半徑，假設(shè)為1個單位。另外，為了不與障礙物發(fā)生碰撞，要求機器人行走線路與障礙物間的最短距離為1個單位，越遠(yuǎn)越安全，否則將發(fā)生碰撞，若碰撞發(fā)生，則機器人無法到達(dá)目標(biāo)點，行走失敗。請回答如下問題：場景圖中有三個目標(biāo)點A(50,40)、B(75,60)、C(95,20),請用數(shù)學(xué)建模的方法給出機器人從R(0,0)出發(fā)安全到達(dá)每個目標(biāo)點的最短路線。求機器人從R(0,0)出發(fā)，依次安全通過A、B到達(dá)C的最短路線。二、問題分析1、問題一中要求求定點R（0,0）按照一定的行走規(guī)則繞過障礙物到達(dá)目標(biāo)點的最短路徑，我們先可以包絡(luò)線畫出機器人行走的危險區(qū)域，這樣的話，拐角處就是一個半徑為1的四分之一圓弧，通過那么然后采用拉繩子的方法尋找可能的最短路徑（比如求R和A之間的最短路徑，我們就可以連接R和A之間的一段繩子，以拐角處的圓弧為支撐拉緊，那么這段繩子的長度便是R到A的一條可能的最短路徑），然后采用窮舉法列出R到每個目標(biāo)點的可能路徑的最短路徑，然后比較其大小便可得出日到目標(biāo)點的最短路徑。2、問題二中要求求定點R（0，0）經(jīng)過中間的若干點按照一定的規(guī)則繞過障礙物到達(dá)目標(biāo)點，這使我們考慮就不僅僅是經(jīng)過障礙物拐點的問題，也應(yīng)該考慮經(jīng)過路徑中的目標(biāo)點處轉(zhuǎn)彎的問題，這時簡單的線圓結(jié)構(gòu)就不能解決這種問題，我們在拐點及途中目標(biāo)點處都采用最小轉(zhuǎn)彎半徑的形式，也可以適當(dāng)?shù)淖儞Q拐點處的拐彎半徑，使機器人能夠沿直線通過途中的目標(biāo)點，然后建立優(yōu)化模型對這兩種方案分別進(jìn)行優(yōu)化，最終求得最短路徑。三、模型假設(shè)1、假設(shè)障礙物全是矩形。2、假設(shè)機器人能夠抽象成點來處理。四、符號說明符號符號說明L路徑的總長度diljr第i段切線的長度第j段圓弧的長度轉(zhuǎn)彎半徑k障礙物上的任意點與行走路徑之間的最短距離五、模型的建立5.1先來證明一個猜想：猜想一：具有圓形限定區(qū)域的最短路徑是由兩部分組成的：一部分是平面上的自然最短路徑（即直線段），另一部分是限定區(qū)域的部分邊界，這兩部分是相切的，互相連接的。（即問題分析中的拉繩子拉到最緊時的狀況）證明：假設(shè)在平面中有A(a,0)和B(-a,0)兩點，中間有一個半圓形的障礙物，證明從A到B的最路徑為A^B。平面上連接兩點最短的路徑是通過這兩點的直線段，但是連接兩點的線段于障礙物相交，所以設(shè)法嘗試折線路徑。在y軸上取一點C(0，y)，若y適當(dāng)大，則折線ACB與障礙物不相交，折線ACB的長度為：IACB\=2<a2+y2顯然IACBI隨著y的減小而減小，減小y得y-y，即C—C，使得AC與CB與1111障礙物相切，切點分別為E和F,顯然ACB是這種折線路徑中最短的。由于滿足10<。<；的角滿足cc<Mnct,所以易知弧度EF小于ECF的長，即ST<^F,從而AE+m+FB<ACB,記線段AE、弧度EF、線段FB為AEFB,那么AEFB比任何折線1路徑都短。下面在考察一條不穿過障礙物的任何一條路徑，設(shè)其分別于OE和OF的延長線交與P、Q兩點，記A和P之間的路徑長度為二，顯然二>|AP|,又由AE±EO,所以|AP|>AE,從而二〉A(chǔ)E,同理可得m；>BF。再來比較PQ之間路徑長度；一；和圓弧EF的長度的大小。若PQ之間的路徑可有極坐標(biāo)方程"?，則有：：:',可得:：—"二?「二一-M3亦即路徑APQB的長度超過路徑AEFB的長度。以上證明足以說明了AEFB是滿足條件A到B的最短路徑。5.2模型準(zhǔn)備一有了4.1中的定理，我們就可以這樣認(rèn)為，起點到目標(biāo)點無論中間障礙物有多少，最短路徑都應(yīng)該是若干個線圓結(jié)構(gòu)所組成。在本題中存在障礙物的狀況，且障礙物在拐點處的危險區(qū)域是一個半徑為1的圓弧，所以結(jié)合定理一，我們易知，求兩點之間的最短路徑中的轉(zhuǎn)彎半徑我們應(yīng)該按照最小的轉(zhuǎn)彎半徑來算才能達(dá)到最優(yōu)。線圓結(jié)構(gòu)5.211）如上圖，設(shè)A（二_，為起點，B（二.二）為目標(biāo)點，C（二.二）和D（二一，「_）分別為機器人經(jīng)過拐點分別于隔離危險線拐角小圓弧的切點，圓心為O（二.，圓的半徑為r，AB的長度為a，AO的長度為b，BO的長度為c，角度ZAOB=cc,ZAOC鄧，ZBOD=「，/COD=0.求ACBB的長度，設(shè)為L.解法如下：如上圖可得有以下關(guān)系:a=寸(x-x)2+(y-y)2TOC\o"1-5"\h\z*2121*b=q(x-x)2+(y-y)25151c=J(x-x)2+(y-y)2〔￥5252在AA08中:,b2+c2-a2、a=arccos()2bc在RtAAOC:rP=arccos-b在RtAAOC中:ry=arccos-c所以:從而可得：L=i：'b2-r2+\，'c2-r2+r02）而對于下圖兩種情況我們不能直接采用線圓的結(jié)構(gòu)來解決，需要做簡單的變換。情況一：

線圓結(jié)構(gòu)5.22我們假設(shè)兩圓心坐標(biāo)分別為03,J）和,J），半徑均為r,M點坐標(biāo)為1122（%,七），那么我們很容易可以求得:X=匚232這樣我們就可以利用1）中的方法，先求A到M，再求M到B，這樣分兩段就可以求解。同理如果有更多的轉(zhuǎn)彎，我們同樣可以按照此種方法分解。情況二：線圓結(jié)構(gòu)5.231122這里我們依然設(shè)圓心坐標(biāo)分別為0（x,j）和0'（x,j），半徑均為r，這樣我1122K00K00—X-X那么00'直線方程為:"K00'（X-X1）+j1因為公切線DE與00平行，那么DE的直線方程可以表示為:其中:C=尸\：'1+七2那么把公切線的方程于圓的方程聯(lián)立，渴可以求得切點D和E的坐標(biāo)。這樣用D和E任意一點作為分割點都可以將上圖分割成兩個4.21所示的線圓結(jié)構(gòu)，這樣就可以對其進(jìn)行求解。同理多個這樣的轉(zhuǎn)彎時，用同樣的方法可以進(jìn)行分割。5?3模型準(zhǔn)備二一、對于從起點經(jīng)過若干點然后再到達(dá)目標(biāo)點的狀況，因為不能走折線路徑，我們就必須考慮在經(jīng)過路徑中的一個目標(biāo)點時轉(zhuǎn)彎的狀況。為了研究這個問題的方便，我們先來證明一個猜想：猜想二：如果一個圓環(huán)可以繞著環(huán)上一個定點轉(zhuǎn)動，那么過圓環(huán)外兩定點連接一根繩子，并以該圓環(huán)為支撐拉緊繩子，達(dá)到平衡狀態(tài)時，圓心與該頂點以及兩條切線的延長線的交點共線。圖5.31證明猜想：如圖4.31所示，E點就是圓環(huán)上的一個頂點，A赤B就是拉緊的繩子，02就是切線AC和BD的延長線的交點，證明O「E、O2三點共線。我們可以用力學(xué)的知識進(jìn)行證明，因為是拉緊的繩子，所以兩邊的繩子拉力相等，設(shè)為F，它們的合力設(shè)為孔，定點對圓環(huán)的作用力設(shè)為F1。那么由幾何學(xué)的知識我們可以知道F一定與qq共線，而又由力的平衡條件可知:F=-FTOC\o"1-5"\h\z01>>-即OO與EO共線。22

綜上所述q、e和q三點一定共線。二、有了以上這個定理我們可以建立以下模型：如圖4.32，要求求出機器人從A繞過障礙物經(jīng)過M點到達(dá)目標(biāo)點B的最短路徑，我們采用以下方法：用一根釘子使一個圓環(huán)定在M點，使這個圓環(huán)能夠繞M點轉(zhuǎn)動。然后連接A和B的繩子并以這些轉(zhuǎn)彎處的圓弧為支撐（這里轉(zhuǎn)彎處圓弧的半徑均按照最小轉(zhuǎn)彎半徑來計算），拉緊繩子，那么繩子的長度就是A到B的最短距離。我們可以把路徑圖抽象為以下的幾何圖形。下面我們對這段路徑求解：圖5.32圖5.32O(x,j)和O(x,j)是兩個固定如圖，A3,J）是起點，B3,J）是終點，的圓，O2是一個可以繞M（p，q）點轉(zhuǎn)動的圓環(huán)，三個圓的半徑均為r，C、D、E、F、G、H均為切點。a、b、c、e，f分別是AO、OO、AO、AO、OO的長1122323度。O(x,j)和O(x,j)是兩個固定L=|AC|+E+|de|+M+|fg|+M+|hb|因為O2點的坐標(biāo)未知，所以我們就不能用模型一中的線圓結(jié)構(gòu)對其進(jìn)行求解。故得先求出O點的坐標(biāo)。設(shè)O坐標(biāo)為（m,n）,AAC、ZAOO、ZAOO、211221ZAOO、ZOOF分別為a(i=1、2、3、4、5),ZCD、ZEOF、ZEOM2332i122分別為氣、七、9。這樣便有以下關(guān)系:a=J(x—x)2+(y—y)2*1313b=扣③—m)2+(y3—n)2<c=J(x—m)2+(y—n)2e=q(x—x)2+(y—y)2414f=\l(x4—m)2+(y4—n)2在RtAAOC中:1ra1=arccos—在AAOO中:12a2+b2一c2a=arccos^^b2+c2一a2a=arccos妥在AAOO中:23c2+f2—e2a=arccos2-f在RtANOF中:22ra=arccosf則：9=空—a—a12121A3兀9=—a—a—a22345又因為MO2一定會在ZEO2F的角平分線上，所以滿足:七我們采用向量的形式來求，易知qq的一個方向向量:I=(i,L)1x-n??而OE與OO垂直，故其一個方向向量:212而:>OM-(p-m,q-n)2所以:—?*-l?OM0=arccos~2,112IIO2MI綜合以上式子可以求得O2的坐標(biāo)，從而可以得出路徑的長為:L=Ja2—r2+0r+b+0r+2,氣）2—r2+11。二麗+HB，這可以采用模型一中的線圓結(jié)構(gòu)來求解。5.4模型準(zhǔn)備三求解從起點經(jīng)過若干個點再到達(dá)目標(biāo)點的問題，與4.4不同，我們還可以有另一種方案，即適當(dāng)擴大障礙物拐點處的拐彎半徑使機器人能夠沿直線通過路徑中的目標(biāo)點。這樣拐點處拐彎圓弧的半徑和圓心都是個變量，對于該題，那么我們可以首先設(shè)定三個圓心q、O2、O3，然后按照以下步驟進(jìn)行作圖：1）給定R，以O(shè)為圓心，R為半徑，畫圓，然后過R點做圓O的切線L，切TOC\o"1-5"\h\z11111點為D。然后過A點做O］的切線設(shè)為L2，切點為E。2）然后做OF垂直于L，垂足為F，OF的長就是R，然后以O(shè)為圓心，R為222222半徑畫圓。很顯然R能由R來確定，即R=f（R）。2121

3）然后過B點做O的切線為L，切點為G,再過OF垂直于L，垂足為H,那TOC\o"1-5"\h\z2333么OH的長度就是R，然后以O(shè)為圓心，R為半徑，畫圓。很顯然R能3333由R2來確定，即R3=f（R2）。4）過C做O3的切線。這就完成了由R經(jīng)過A和B在到達(dá)C的路徑。5）然后再變換O、O、O、R、R、R，可得到新的路徑。找出最小者即可。1231235.5模型的建立假設(shè)機器人從起點R到到目標(biāo)點M0，由4.2知路徑一定是由圓弧和線段組成，設(shè)有m條線段，n條圓弧。那么目標(biāo)函數(shù)可以表示為：minL=￡d.+?i=1j=1s.t=<r>1s.t=<用此模型就可以對起點到目標(biāo)點之間的路徑進(jìn)行優(yōu)化求解。六、模型的求解6.1問題一一、以下給出了日到個目標(biāo)點的可能路徑的最短路徑：1）如圖一，解決的就是R到目標(biāo)點A的最短路徑問題，很顯然的一個問題是機器人從三：的上方走的最短路徑肯定是大于機器人從三-下方走的最短路徑，所以機器人從《方向走的最優(yōu)路線我們在圖一中沒有給出。圖一中，藍(lán)線就可以為機器人隔出了危險區(qū)域，紅線表示的就是通過"點的圓為支撐而拉緊的繩子，這樣計算出繩子的長度就是R到A的最短路徑。2）如圖二，解決的是R到目標(biāo)點B的最短路徑問題，圖中給出了可能的三條路徑的最短路徑（圖中的紅線所示），我們可以分別計算出三條可能路徑的最短路徑的長度，然后進(jìn)行比較，取最小者就是R到目標(biāo)點B的最優(yōu)路徑。3）如圖三，解決的是R到目標(biāo)點C的最短路徑問題。圖中給出了兩條可能路徑的最短路徑（圖中的紅線所示），我們同樣可以分別計算出兩條可能的最短路徑，取最小者就是R到目標(biāo)點C的最優(yōu)路徑。

圖6.13二、然后用matlab求解結(jié)果如下：R到目標(biāo)點A的最短距離為70.5076R到目標(biāo)點B的可能路徑有三條，即就有三條可能的最短路徑。如圖二，機器人走最上面這條路徑到達(dá)B，直接用matlab求得最短路徑為114.1611而機器人經(jīng)過中間一條路徑到達(dá)B,這條路徑由三條線段和兩段圓弧組成，直接用三中的解法是結(jié)不出來的。于是我們做了如下變換，先求出中間一條直線的中點設(shè)為F，那么可以采用三中的解法，分別求R到F和F到B的最短路徑，然后兩段相加，便可求出R到B的最短路徑。求解結(jié)果為107.9587機器人經(jīng)過最下邊一條路徑，同理這條路徑由四條直線和三個圓弧組成，同樣可以采?、谥械淖儞Q，分三部分求解。求解結(jié)果如下為116.1256綜合①②③所述，日到目標(biāo)點B的最短路徑為107.9587R到目標(biāo)點C的可能路徑由兩條，和2中的方法一樣，最終求解結(jié)果R到目標(biāo)點C的最短路徑為102.05146.2問題二一、我們先按照4.3中的思想來進(jìn)行求解，這樣我們可以做出5.21所示的示意圖:顯然運用模型準(zhǔn)備二中的方法求解的結(jié)果小于用模型準(zhǔn)備三中的方法求解的結(jié)果，說明模型準(zhǔn)備二的方法優(yōu)于模型準(zhǔn)備三的方法。七、模型推廣7.1問題深入分析在本題中只有四個障礙物，按照線圓結(jié)構(gòu)畫出從起點到達(dá)目標(biāo)點的路徑是有限的，我們完全可以采用窮舉法把這些路徑列出來，然后比較大小取最小者即可，但是我們可以設(shè)想如果這個區(qū)域內(nèi)有n個障礙物，那么按照線圓結(jié)構(gòu)從起點到達(dá)目標(biāo)點的可能路徑就有無數(shù)多條，這時我們?nèi)绻诓捎酶F舉法是不現(xiàn)實的。所以我們必須尋求新的算法來解決這個問題。由上述分析我們有了這樣一個想法：先求出所有的切線，包括出發(fā)點和目標(biāo)點到所有圓弧的切線以及所有圓弧與圓弧之間的切線，然后把這且曲線看成是圖6.11中的，給這些定點賦一個等于切線長度的權(quán)值，如果某兩條切線有一個公切圓弧，則代表這兩條曲線的頂點是一條直線的兩個端點，邊上的權(quán)值等于這兩條切線之間的劣弧長度。然后在這張圖中加一個源點和終點，那么在所有代表出發(fā)點與其它圓弧之間切線的頂點與源點連成一條邊，權(quán)值均為0，同理在所有代表目標(biāo)點到其它圓弧切線的頂點與終點連成一條邊，權(quán)值均為0，這樣題目就轉(zhuǎn)化成了求源點到達(dá)終點之間的最短路徑問題了，這里最短路徑就是指經(jīng)過所有頂點與邊的權(quán)值之和最小。我們可以采用Dijkstra算法進(jìn)行求解。7.2模型的進(jìn)一步求解根據(jù)6.1的分析，在有若干個障礙物的區(qū)域中，我們把按照線圓結(jié)構(gòu)畫出從出發(fā)點到目標(biāo)點的路徑圖依據(jù)6.1中的想法轉(zhuǎn)換成了下面這張圖，圖中的A和G點就是添加的源點和終點，其它節(jié)點均是出發(fā)點和目標(biāo)點到圓弧的切線和圓弧與圓弧之間的切線轉(zhuǎn)化而成。圖7.21對于最短路徑的求解，有以下步驟：1）我們畫出出發(fā)點和目標(biāo)點和各個圓弧的切線，以及圓弧與圓弧之間的切線，但是切線有可能經(jīng)過障礙物的內(nèi)部或危險區(qū)域，也可能出現(xiàn)切線重復(fù)的狀況，既有很多不合法的切線。于是我們對模型做了以下修正：1、檢驗切線兩個端點是否在障礙物內(nèi)部。2、檢驗切線是否障礙物的對角線相交。3、檢驗圓弧所對應(yīng)的圓心，即障礙物的頂點到切線的距離是否小于1。如果以上三種情況滿足其一，我們規(guī)定對應(yīng)這段切線的頂點為M（M為無窮大）。4、另外還有如下圖所示的一種特殊情況：兩個大小相同在同一水平或者豎直位置上，不考慮切線滿足1、2、3的狀況它們由2條內(nèi)公切線，8條外公切線，但是有6條外公切線是重復(fù)的。因此我們作如下規(guī)定：如果某條切線與某段圓弧相切，且切點不在切線的端點上，則該切線為不合法。權(quán)值矩陣中表示它的頂點也為M。ABCD圖7.222）然后把合法的切線與這些切線之間的劣弧轉(zhuǎn)化成如7.21所示的形式。假設(shè)轉(zhuǎn)化過后有m條合法切線，那么就有m個頂點，設(shè)這些點的權(quán)值凡（1<i<m），即第i條合法曲線的長度。。/.為邊的權(quán)值，即第/條弧的長度。3）然后把路徑圖轉(zhuǎn)化成如圖6.21所示，按照求得權(quán)值矩陣給圖中的頂點及邊長賦值。4）最后依據(jù)Dijkstra算法求得最短路徑。八、模型評價一、模型優(yōu)點1、運用多個方案對路徑進(jìn)行優(yōu)化，在相對優(yōu)化之中取得最優(yōu)解。2、模型優(yōu)化后用解析幾何進(jìn)行求解，精確度較高。3、模型簡單易懂，便于實際檢驗及應(yīng)用。二、模型缺陷1、此模型適于全局規(guī)劃，獲得精確解卻失去了效率。2、在障礙物較多時，且形狀不規(guī)則時，模型需要進(jìn)一步改進(jìn)。九、參考文獻(xiàn)譚永基，數(shù)學(xué)模型，上海，復(fù)旦大學(xué)出版社，2011邦迪，圖論及其應(yīng)用，西安，西安科學(xué)出版社1984胡海星，RPG游戲中精靈的移動問題，雜志《程序員》2011；尤承業(yè)，解析幾何，北京，北京大學(xué)出版社，2004周培德，計算幾何一算法與設(shè)計，北京清華大學(xué)出版社，2005十、附錄%求解一次轉(zhuǎn)彎所經(jīng)路線總長%T：初始點V:轉(zhuǎn)彎圓弧圓心W:到達(dá)點functionresult=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((TV"2+VW"2-TW”2)/(2*TV*VW));alpha2=acos(r/TV);alpha3=acos(r/VW);alpha4=2*pi-alpha1-alpha2-alpha3;%alpha4為轉(zhuǎn)彎圓心角TS1二sqrt(TV"2-