【文章內(nèi)容簡(jiǎn)介】 0,600)amp。gt。(730,520)amp。gt。(,)amp。gt。(,)amp。gt。(,)amp。gt。(,)amp。gt。(,)amp。gt。(,)amp。gt。(,)amp。gt。(0,0)經(jīng)過(guò)的圓心：(410,100)， (230,60)， (80,210)， (220,530)， (150,600)， (270,680)， (370,680)， (430,680)， (670,730)， (540,730)， (720,520)， (720,600)， (500,200) 最短路長(zhǎng)為：。本次求解，在搜索可行路徑上程序運(yùn)行時(shí)間為10分鐘左右。 對(duì)每條路徑，直接根據(jù)避障條件，判斷它是否可行路徑的方法——基于數(shù)值近似的線段間最短距離判別法。線段間最短距離判別法：給定路徑端點(diǎn)坐標(biāo)，所有障礙物邊界線段的坐標(biāo)，分別在路徑和某一邊界線段上取一系列的點(diǎn)，求得這系列點(diǎn)之間的最短距離即為線段間最短距離的近似值，如果該近似值大于10，則認(rèn)為路徑與該邊界線段的距離滿足要求，否則不滿足。直到該路徑與所有的邊界線段都滿足距離要求，則該路徑為可行路徑并記錄。易知，若線段間的一系列點(diǎn)取值越細(xì)密，結(jié)果越精確。按照該算法，我們編寫(xiě)MATLAB程序得到的所有可行路徑如下圖：18 對(duì)此可行路徑集合，仍然使用基于避障條件轉(zhuǎn)化之下的最短路徑的啟發(fā)算法的StepStep3步尋找最短路徑，最短路徑結(jié)果完全等同基于避障條件轉(zhuǎn)化之下的最短路徑的啟發(fā)算法的結(jié)果，但是求解時(shí)間卻大大縮減，說(shuō)明該算法具有一定的有效性。 六、最短時(shí)間路徑模型建立與求解基于問(wèn)題一轉(zhuǎn)化為最短路徑的優(yōu)化問(wèn)題后，我們易知優(yōu)化的目標(biāo)函數(shù)為機(jī)器人在行進(jìn)過(guò)程中最短時(shí)間路徑，通過(guò)01變量來(lái)選取經(jīng)過(guò)的轉(zhuǎn)彎點(diǎn)的圓心坐標(biāo)，兩切點(diǎn)坐標(biāo)及轉(zhuǎn)彎半徑，此時(shí)將轉(zhuǎn)彎點(diǎn)的圓心坐標(biāo)，兩切點(diǎn)坐標(biāo)及轉(zhuǎn)彎半徑均作為決策變量。因此，可建立01規(guī)劃模型如下。目標(biāo)函數(shù)為機(jī)器人出區(qū)域中一點(diǎn)到達(dá)另一點(diǎn)的避障最短時(shí)間路徑，避障最短路徑Z為行進(jìn)過(guò)程中直線路徑與轉(zhuǎn)彎路徑的時(shí)間取和，故有minZ=229。229。Xij(i=1j=1nnaijv0+bijvr) （8）其中，aij為切點(diǎn)i到切點(diǎn)j的直線距離；bij為機(jī)器人從切點(diǎn)i行進(jìn)至j切點(diǎn)時(shí)的轉(zhuǎn)彎路徑；Xij為01變量，表示若選擇行走切點(diǎn)(xi,yi)后行走切點(diǎn)(xj,yj)，Xij為1，否則為0，i,j=1...n,n=36,i185。j。機(jī)器人從切點(diǎn)i到切點(diǎn)j的直線路徑aij為19aij=xi2yi2+(xj300)2+(yj300)2機(jī)器人從切點(diǎn)i行進(jìn)至j切點(diǎn)時(shí)的bij為 其中bij=qrki,j=1..36,i185。j i,j=1..36,i185。j （9） (xixj)2+(yiyj)2=ri+ri2ricosq 2（10） rk為轉(zhuǎn)彎半徑。原理與最短路徑的避障條件的原理保持一致。所有假設(shè)的坐標(biāo)點(diǎn)都應(yīng)在800180。800平面場(chǎng)景 xi2163。xj1yi2163。xj1 其中，i,j=1..36,i185。j。、圓心、與切點(diǎn)的坐標(biāo)關(guān)系。轉(zhuǎn)彎半徑等于圓心(x0,y0)與兩個(gè)切點(diǎn)(xi,yi),(xj,yj)坐標(biāo)的直線距離。rk=(x0xi)2+(y0yi)2rk=(x0x22j)+(y0yj)aij5. 轉(zhuǎn)彎半徑rmin(i取值為：使v+bij0v)r滿足的r20 11） （綜合上述，建立避障最短時(shí)間路徑模型是在：minZ=229。229。Xij(i=1j=1nnaijv0+bijvr)236。239。239。Yij179。Xij239。239。xi1163。xi2239。yi1163。yi2239。239。yj2163。yj2239。x163。xj1239。i2239。yi2163。=qrki,j=1..36,i185。j239。222239。(xixj)+(yiyj)=rk+rk2rkcosq239。x,x,x,y,x,y,y,x,x,y,y206。(0,800)pp239。i1i2j1i1i1i2j1p239。2222a=xy+(x300)+(y300)ijiijj239。239。X,Y=0或1（i,j=1,2,...n,n163。36,i185。j）239。ijij239。r=(xx)2+(yy)20i0i239。k239。22r=(xx)+(yy)239。k0j0j238。 amp。gt。A的最短時(shí)間路徑的簡(jiǎn)化模型針對(duì)只求0amp。gt。A的最短時(shí)間路徑，我們具體問(wèn)題具體分析，建立了相應(yīng)的簡(jiǎn)化模型。 障礙物5左上角的頂點(diǎn)與圓弧的距離大于等于10的避障約束可以表述如下：d=ri(80x0)2+(210y0)2179。10于是，我們可得一條路徑時(shí)間最短下的最優(yōu)r的簡(jiǎn)化模型minZ=229。229。Xij(i=1j=1nnaijv0+bijvr)236。d=r(80x)2+(210y)2179。10i00239。i,j=1..36,i185。j239。bij=qri239。222239。(xixj)+(yiyj)=ri+239。xi1,xi2,xj1,yi1,xi1,yi2,yj1,xp,x,yp,yp206。(0,800)239。2222i,j=1..36,i185。j239。aij=xixj+(xj300)+(yj300)239。238。Xij=0或1（i,j=1,2,...n,n163。36,i185。j）21針對(duì)每一條可行路徑，求出各路徑對(duì)應(yīng)的最優(yōu)轉(zhuǎn)彎半徑ri(i=1,2)則，最短時(shí)間路徑的優(yōu)化模型為：236。aijbijnnaijbij252。239。nn239。Z=min237。229。229。Xij(+),229。229。Xij(+)253。 vvvv239。239。i=1j=1i=1j=10r0r12254。238。amp。gt。A的最短時(shí)間路徑的模型求解根據(jù)0amp。gt。A的最短時(shí)間路徑的模型，利用LINGO軟件編程，比較兩條可行路徑對(duì)應(yīng)的最短時(shí)間， 。相應(yīng)圓弧的圓心坐標(biāo)為（，），兩切點(diǎn)坐標(biāo)分別為（，）、（,）。七、模型評(píng)價(jià)（1）確定路線思路循序漸進(jìn)，（2）給出了二種啟發(fā)式算法，最短路徑即最短時(shí)間路徑具有一定可信度。同時(shí)第一個(gè)啟發(fā)算法可以求得全局最優(yōu)解，第二個(gè)啟發(fā)算法是針對(duì)問(wèn)題的NP屬性減少求解時(shí)間而構(gòu)建的，兩個(gè)算法都具有較重要的意義。（1）（2）模型將假設(shè)機(jī)器人在行進(jìn)、轉(zhuǎn)彎過(guò)程中能一直保持最大的速度，誠(chéng)然，現(xiàn)實(shí)并非如此，所以我們得到的問(wèn)題二的結(jié)果與實(shí)際最短時(shí)間路徑會(huì)存在一定的誤差。參考文獻(xiàn)[1] [M], 北京：中國(guó)水電出版社，2006，210[2] [M]，北京：國(guó)防工業(yè)出版社，[3] [M].北京: 北京航空航天大學(xué)出版社，2003,3 22附錄 MATLAB程序清單：clear …………………………計(jì)算兩線段間近似最短路徑 function mind=distancebetweenlines(A,B,C,D,M)Ax=A(1)。Ay=A(2)。Bx=B(1)。By=B(2)。Cx=C(1)。Cy=C(2)。Dx=D(1)。Dy=D(2)。if (A(1)B(1))~=0k=(ByAy)/(BxAx)。b=Byk*Bx。ABXXL=linspace(A(1),B(1),M)。ABYXL=k.*ABXXL+b。elseABXXL=linspace(A(1),B(1),M)。ABYXL=linspace(A(2),B(2),M)。endif (C(1)D(1))~=0k=(DyCy)/(DxCx)。b=Dyk*Dx。CDXXL=linspace(C(1),D(1),M)。CDYXL=k.*CDXXL+b。elseCDXXL=linspace(C(1),D(1),M)。CDYXL=linspace(C(2),D(2),M)。endmind=100000。for i=1:Mfor j=1:Mif sqrt((ABXXL(i)CDXXL(j))+(ABYXL(i)CDYXL(j)))amp。lt。=mind mind=sqrt((ABXXL(i)CDXXL(j))+(ABYXL(i)CDYXL(j)))。 endendend ?????????????Folyd算法求最短路徑和最短路程 function [L,R]=FLOYD1(w,s,t)n=size(w,1)。D=w。path=zeros(n,n)。%210。212。207。194。202。199。177。234。188。floyd203。227。168。for i=1:n23for j=1:nif D(i,j)~=infpath(i,j)=j。endendendfor k=1:nfor i=1:nfor j=1:nif D(i,k)+D(k,j)amp。lt。D(i,j)D(i,j)=D(i,k)+D(k,j)。path(i,j)=path(i,k)。endendendendL=zeros(0,0)。R=s。while 1if s==tL=fliplr(L)。L=[0,L]。returnendL=[L,D(s,t)]。R=[R,path(s,t)]。s=path(s,t)。end ?????????????已知圓心和圓上2點(diǎn)，計(jì)算弧長(zhǎng) function z=huchang(A,B,C,r)%C206。170。212。178。208。196。248。177。234。%A,B206。170。212。178。187。161。182。203。181。227。248。177。234。,r206。170。176。235。190。182。D=dot(AC,BC)/(norm(AC)*norm(BC))。%163。228。163。239。163。244。207。242。193。191。181。227。187。253。theta=acos(D)。%187。161。182。200。177。237。202。190。z=theta*r。 ?????????????判斷線段是否和圓相交 function z=iscycleIntersect(line,A,r)if (line(3)line(1))~=0k=(line(4)line(2))/(line(3)line(1))。b=line(2)k*line(1)。d=abs(k*A(1)A(2)+b)/sqrt(k+1)。[x y]=solve([‘x*(‘ num2str(k) ‘)y+(‘ num2str(b) ‘)=0’],...[‘(y(‘ num2str(A(2)) ‘))*(‘ num2str(k) ‘)=(‘ num2str(A(1)) ‘)x’])。 else24x=line(3)。y=A(2)。d=abs(line(3)A(1))。endx=double(x)。y=double(y)。if damp。gt。=z=0。elseif (yline(4))*(yline(2))amp。gt。0z=0。elsez=1。end ?????????????判斷兩線段是否相交 function z=islineIntersect(A,B,C,D)Ax=A(1)。Ay=A(2)。Bx=B(1)。By=B(2)。Cx=C(1)。Cy=C(2)。Dx=D(1)。Dy=D(2)。if ((BxAx)*(DyCy)(ByAy)*(DxCx))*((BxAx)*(DyCy)(ByAy)*(DxCx))~=0 r=((AyCy)*(DxCx)(AxCx)*(DyCy))/((BxAx)*(DyCy)(ByAy)*(DxCx))。 s=((AyCy)*(BxAx)(AxCx)*(ByAy))/((BxAx)*(DyCy)(ByAy)*(DxCx))。 if ramp。gt。0amp。amp。amp。amp。ramp。lt。=1amp。amp。amp。amp。samp。gt。0amp。amp。amp。amp。samp。lt。=1z=1。elsez=0。endelsez=0。end ?????????????判斷切點(diǎn)是否位于可行圓弧上 function z