何改平
(1.西安外事學(xué)院 工學(xué)院,陜西 西安710077;2.西安電子科技大學(xué) 數(shù)學(xué)與統(tǒng)計(jì)學(xué)院,陜西 西安710071)
隨著計(jì)算機(jī)、微電子技術(shù)的不斷發(fā)展,智能機(jī)器人成為眾多學(xué)者研究的熱點(diǎn).現(xiàn)有文獻(xiàn)針對(duì)機(jī)器人的路徑規(guī)劃研究較多,主要分為全局和局部路徑規(guī)劃[1-7],采用先進(jìn)的模糊控制理論、神經(jīng)元控制理論等進(jìn)行路徑的最優(yōu)規(guī)劃,算法相對(duì)復(fù)雜[8].本文主要建立沖突檢查模型,對(duì)機(jī)器人在限定區(qū)域內(nèi)順利避開(kāi)障礙物的最優(yōu)路徑進(jìn)行分析,結(jié)合MATLAB仿真計(jì)算工具,分情況進(jìn)行實(shí)例計(jì)算驗(yàn)證了模型的正確性.
圖1 不同形狀障礙物平面圖
假設(shè)機(jī)器人在一個(gè)800m×800m場(chǎng)景圖的原點(diǎn)O(0,0)處,而且它只能在該區(qū)域內(nèi)活動(dòng).其內(nèi)有12個(gè)使機(jī)器人不能與之發(fā)生碰撞的不同形狀的障礙物,障礙物的數(shù)學(xué)描述如圖1所示.
在平面場(chǎng)景圖1中,機(jī)器人要到達(dá)障礙物外指定的目標(biāo)點(diǎn)O(0,0),A(300,300),B(100,700),C(700,640)(目標(biāo)點(diǎn)與障礙物之間的距離至少超過(guò)10m).規(guī)定機(jī)器人的行走路徑由直線段和圓弧組成,其中圓弧是機(jī)器人轉(zhuǎn)彎路徑.因?yàn)闄C(jī)器人不能折線轉(zhuǎn)彎,只能通過(guò)圓弧與其相切直線或圓弧與其相切圓弧來(lái)完成轉(zhuǎn)彎,所以機(jī)器人的行走路線是由直線和圓弧所構(gòu)成的.而每個(gè)圓弧的半徑最小為10m,且規(guī)定機(jī)器人與障礙物的最小距離為10m.若無(wú)法保證此規(guī)定,機(jī)器人會(huì)因與障礙物發(fā)生碰撞而無(wú)法完成行走.機(jī)器人直線行走最大速度和最大轉(zhuǎn)彎速度分別為v0=5m/s;v=v(ρ)=v0/(1+exp(10-0.1ρ2)),其中,ρ為半徑.如超過(guò)該速度,機(jī)器人將會(huì)側(cè)翻無(wú)法行走.
(1)假設(shè)機(jī)器人可以用抽象點(diǎn)來(lái)說(shuō)明.
(2)假設(shè)問(wèn)題一中機(jī)器人經(jīng)過(guò)障礙物時(shí)拐角處均是一個(gè)半徑為10m圓弧.
(3)假設(shè)機(jī)器人行走過(guò)程當(dāng)中均以最大速度行駛且不會(huì)出現(xiàn)故障.
(4)假設(shè)機(jī)器人行走速度突變時(shí)沒(méi)有緩沖.
沖突的檢查過(guò)程,即查看路徑距離障礙物的最短距離是否符合機(jī)器人距離障礙物的最短距離要求.由于路徑由線段和弧線組成,現(xiàn)分別建立模型.
Step 1:檢查路徑中的所有線段是否滿足距離要求,如果不滿足,直接返回false.
Step 2:檢查路徑中的弧線是否滿足距離要求,如果不滿足,直接返回false.
Step 3:如果Step 1和Step 2都滿足,返回true,說(shuō)明該路徑是一個(gè)有效的路徑.
2.1.1 線段的檢查 (1)線段與多邊形的檢查.①查看該線段的兩個(gè)端點(diǎn)到多邊形的各個(gè)邊的距離是否滿足安全距離要求,如果不滿足直接返回false;②查看多邊形的各個(gè)端點(diǎn)到該線段的距離是否有不滿足的,如果有不滿足的直接返回false;③如果①和②都滿足返回true.
(2)線段與圓之間的檢查.①?gòu)膱A心向線段所在的直線做垂線,如果垂足落在線段上,看垂線段的長(zhǎng)度和圓半徑的差是否滿足距離,如果不滿足返回false;如果垂足落在線段外,計(jì)算線段上靠近垂足的點(diǎn)與圓心的距離,以及這段距離和圓半徑的差是否滿足要求,如不滿足返回false;②如果①滿足返回true.
2.1.2 弧線的檢查 (1)弧線和多邊形的檢查.①查看弧線的端點(diǎn)和多邊形的各個(gè)端點(diǎn)是否滿足距離要求,如果不滿足返回false;②從多邊形的各個(gè)點(diǎn)向圓心做線段,如果該線段和弧線沒(méi)有交點(diǎn),忽略;如果該線段的長(zhǎng)度與弧半徑的差不滿足距離要求返回false;③從弧心向多邊形的各個(gè)邊做垂線,如果垂線和弧線相交并且垂足落在邊上,計(jì)算垂線段與弧半徑的差是否滿足距離要求,否則忽略.
(2)弧線與圓的檢查.假設(shè)弧線L的半徑為r1,圓O的半徑為r2.①?gòu)幕⌒牡綀A心做線段L1,如果弧線L與線段L1相交,檢查L(zhǎng)1-r1-r2是否滿足距離要求;②如果L與L1不相交,計(jì)算L的端點(diǎn)與圓O的圓心之間的距離S,查看s-r2是否滿足要求.
2.2.1 點(diǎn)到圓的切點(diǎn)距離 如圖2(a)所示,設(shè)點(diǎn)的坐標(biāo)為(a,b),如果切線斜率存在,則設(shè)為k,可得切線方程為y-b=k(x-a),圓的方程為(x-m)2+(y-n)2=r2,根據(jù)圓心到切線的距離等于半徑可得
其中,a,b,m,n已知,可求得斜率k,進(jìn)而求得切線方程y=k(x-a)+b,然后將切線方程和圓的方程聯(lián)立,即
解方程便可求出切點(diǎn)坐標(biāo)(x1,x2),由兩點(diǎn)距離公式求得點(diǎn)到圓的切點(diǎn)距離l.
2.2.2 切點(diǎn)到切點(diǎn)間圓弧的距離 用沖突檢查模型準(zhǔn)備一中的方法求出2個(gè)切點(diǎn)坐標(biāo),分別為A(x1,y1),B(x2,y2),若圓的半徑為r,如圖2所示,則弦長(zhǎng)d和夾角θ分別為
圖2 切點(diǎn)到切點(diǎn)間的距離示意圖
再根據(jù)弧長(zhǎng)公式可得弧長(zhǎng)c
2.2.3 切點(diǎn)到切點(diǎn)之間線段的距離 (1)內(nèi)公切線.如圖2(B)所示,EF為內(nèi)公切線,E,F(xiàn)為切點(diǎn),設(shè)切線方程為y=kx+b,圓O的方程為(x-m1)2+(y-n1)2=r2,圓O′方程為(x-m2)2+(y-n2)2=r2,由圓心到切線的距離等于半徑可得方程組
由方程組(7)即可解得k,b,進(jìn)而得出切線方程,再與兩個(gè)圓分別聯(lián)立
求解方程組可解得切點(diǎn)D(x1,y1),E(x2,y2),根據(jù)兩點(diǎn)距離公式得到兩切點(diǎn)線段距離
(2)外公切線.如圖2(C)所示,設(shè)圓心坐標(biāo)分別為O(x1,y1)和O′(x2,y2),半徑均為r,這樣可以得到
那么切線方程為
由內(nèi)切線算法,求出切點(diǎn)、切線方程,進(jìn)而求得切點(diǎn)坐標(biāo)和兩切點(diǎn)之間的長(zhǎng)度.
對(duì)于任何一條機(jī)器人走的線路,其路線都是由線 -弧 -線組成,設(shè)X1,X2,…,X2n,X2n+1分別為路徑上的拐點(diǎn),即直線和弧線的交點(diǎn)(切點(diǎn)).其中奇數(shù)點(diǎn)和偶數(shù)點(diǎn)直接表示直線,偶數(shù)點(diǎn)和奇數(shù)點(diǎn)直接表示弧線,建立模型如下:
問(wèn)題變成求一組能通過(guò)障礙物檢驗(yàn)的X使得S最小.
(1)由圖3得出O→A的兩種路線,路徑A和路徑B.通過(guò)計(jì)算比較得到最短路徑為路徑A.設(shè)切線OX1的方程為y=kx,利用圓心到切線距離等于半徑可得
圖3 O→A的兩種路徑示意圖
其中,x=80,y=210,化簡(jiǎn)可得k1=3.023 0,k2=2.310 3(舍去).聯(lián)立切線方程和圓的方程
可得x=70.506 0,y=213.140 6.即切點(diǎn) X1(70.596 0,213.140 6),同理得出切點(diǎn) X2(76.606 4,219.406 6),現(xiàn)有兩點(diǎn) X1(70.596 0,213.140 6),X2(76.606 4,219.406 6).根據(jù)
得c1=rθ=9.050 9,所以O(shè)→A的最短路徑為
S1=l1+c1+l2,得總長(zhǎng)S1=471.037 2.
(2)O→B的最短路徑:從O到圖形6左下角,到圖形6的頂點(diǎn),到圖形7的右下角,到圖形7的右上角,到圖形8的左下角,到B.即
由問(wèn)題分析可知最短時(shí)間路徑問(wèn)題是在最短路徑問(wèn)題的基礎(chǔ)上,考慮到速度因素,得到最短時(shí)間路徑,故以拐彎半徑為變量,設(shè)為ρ,從O到A由線段OX1,圓弧X1X2,線段X2A組成,設(shè)它們的長(zhǎng)度分別為l1,c,l2,可得最短時(shí)間模型為
圖4 O→B的最短路徑示意圖
圖5 最短時(shí)間路徑
(1)l1求解:設(shè)切線方程為y=kx,圓的方程為(x-80)2+(y-210)2=ρ2,根據(jù)圓心到切線距離等于半徑可得
經(jīng)化簡(jiǎn),據(jù)圖5可得需要斜率稍大的切線,聯(lián)立切線方程和圓的方程
由方程組可求得切點(diǎn)坐標(biāo)為關(guān)于ρ的函數(shù),即P1(x(ρ),y(ρ)),進(jìn)而求得l1
(2)l2求解:設(shè)切線方程為y′-300=k′(x′-300),圓的方程為(x′-80)2+(y′-210)2=ρ2,同樣可求得另一個(gè)切點(diǎn)坐標(biāo)P2(x′(ρ)、y′(ρ)),得到l2
(3)弧長(zhǎng)c求解:由P1(x(ρ),y(ρ)),P2(x′(ρ),y′(ρ))的坐標(biāo)及幾何模型,可直接得到
將求得的l1,c,l2代入目標(biāo)函數(shù),解得ρ=11.502 5時(shí),時(shí)間最短為T(mén)=94.564 9,且最短路徑為O(0,
文中建立的模型算法優(yōu)點(diǎn)在于將起始點(diǎn)到目標(biāo)點(diǎn)分解成多個(gè)線-弧-線模型,使得復(fù)雜問(wèn)題簡(jiǎn)單化,易于實(shí)現(xiàn);利用沖突檢查模型結(jié)合解析幾何模型優(yōu)化后通過(guò)程序分別進(jìn)行求解,精確度較高,便于實(shí)際檢驗(yàn)及應(yīng)用.缺點(diǎn)是重復(fù)計(jì)算量大,效率不高;在障礙物較多,且形狀不規(guī)則時(shí),模型需要進(jìn)一步改進(jìn).針對(duì)模型中有大量重復(fù)計(jì)算的情況,可以利用MATLAB程序進(jìn)行模塊化編程.
機(jī)器人避障模型可以應(yīng)用于電子地圖路線查找,電纜及管線的鋪設(shè),交通運(yùn)輸?shù)?,它們都與最短路徑有關(guān),而這些問(wèn)題用平常方法解決比較困難,所以可用該模型來(lái)幫助人們解決生活中的很多實(shí)際問(wèn)題.
[1]李智也.移動(dòng)機(jī)器人路徑規(guī)劃問(wèn)題的解決方案 [J].計(jì)算機(jī)工程,2006,32(1):189-192.
[2]金秀慧,伊連云,付瑩瑩,等.基于通用運(yùn)動(dòng)學(xué)模型的移動(dòng)機(jī)器人避障路徑規(guī)劃[J].機(jī)械工程師,2005,34(12):34-35.
[3]董宇欣.移動(dòng)機(jī)器人路徑規(guī)劃方法研究[J].信息技術(shù),2006(6):108-111.
[4]王強(qiáng),姚進(jìn),王進(jìn)戈.基于遺傳算法的移動(dòng)機(jī)器人的一種路徑規(guī)劃方法[J].哈爾濱工業(yè)大學(xué)學(xué)報(bào),2004(7):867-870.
[5]楊晶東,楊敬輝,蔡則蘇.基于多目標(biāo)優(yōu)化的移動(dòng)機(jī)器人避障算法[J].上海交通大學(xué)學(xué)報(bào),2012,46(2):213-216.
[6]NUUKAEW Wuttinan,PHRUKSAPHANRAT.A fuzzy multiple objective decision making model for solving a multidepot distribution problem[C]//Proceedings of the international Multi Conference of Engineers and Computer Scientists.Hong kong:IMECS,2010:17-19.
[7]KRUUSMAA M,WILLEMSON J.Covering the path space:A case base analysis for mobile robot path planning[J].Knowledge-Based Systems,2003,16(5/6):235-242.
[8]KIRBY Rachel,SIMMONS Reid,F(xiàn)ORLIZZI Jodi.Companion:A constraint optimizing method for person acceptable navigation[C]//The 18th IEEE International Symposium on robot and Human Interactive Communication(RO-MAN)Toyama,Japan:IEEE,2009:607-612.