專利名稱：基于矢量法的路徑規(guī)劃預(yù)處理方法
技術(shù)領(lǐng)域：
本發(fā)明涉及路徑規(guī)劃預(yù)處理方法，尤其涉及一種基于矢量法的路徑規(guī)劃預(yù)處理方 法，用于仿真技術(shù)領(lǐng)域。
背景技術(shù)：
路徑規(guī)劃(Path Planning)是指按照某個(gè)評(píng)價(jià)標(biāo)準(zhǔn)(如最短路徑長(zhǎng)度、最短行進(jìn) 時(shí)間、最小能量消耗等)，滿足某個(gè)約束條件(如避免與障礙物碰撞等)，規(guī)劃一條從起始點(diǎn) 位置到達(dá)目標(biāo)點(diǎn)位置最優(yōu)(或次優(yōu))的路徑(詳細(xì)內(nèi)容可參考文獻(xiàn)[戴光明.避障路徑規(guī) 劃的算法研究[D].武漢華中科技大學(xué)，2004])。路徑規(guī)劃作為一個(gè)經(jīng)典的人工智能問 題，在仿真、機(jī)器人、無(wú)人駕駛車輛等領(lǐng)域得到了廣泛而深入的研究。目前，在這些領(lǐng)域使用 的路徑規(guī)劃方法主要有圖搜索算法(如A*算法[蘇浩，李欽富，蔡駿.A*算法在基于道路 網(wǎng)的路徑規(guī)劃中的應(yīng)用.中國(guó)電子科學(xué)研究院學(xué)報(bào)，2010，5(4): 419-422.], Dijkstra 算法[WAN Ming, ZHANG Wei, MURRAY Marie 0.，KAUFMAN Arie. Automatic target tracking on multi-resolution terrain. Journal of Zhejiang University, 2006, 7(7): 1275-1281.]、基于軟計(jì)算的規(guī)劃方法(如遺傳算法[梁曉輝，吳威，趙沁平.大規(guī) 模真實(shí)地形數(shù)據(jù)中的全局路徑規(guī)劃方法——基于遺傳算法的研究.計(jì)算機(jī)研究與發(fā)展， 2002，39(3) 301-306.]，蟻群算法[TAN Guan-zheng, HE Huan, Aaron Sloman. Global optimal path planning for mobile robot based on improved Dijkstra algorithm and ant system algorithm. Journal of Central South University of Technology, 2006，13(1) : 80-86]、基于隨機(jī)采樣的規(guī)劃方法(如快速擴(kuò)展隨機(jī)樹(Rapidly-exploring Random Tree，RRT [樊曉平，彭展，張恒，羅熊.基于快速擴(kuò)展隨機(jī)樹的機(jī)器人路徑規(guī)劃仿真 實(shí)驗(yàn)平臺(tái)研究.鐵道科學(xué)與工程學(xué)報(bào)，2005，2(2): 86-92])等。上述方法都建立在一定 的空間數(shù)據(jù)結(jié)構(gòu)表示方法之上，常用的有矢量法和柵格法。在仿真系統(tǒng)中，實(shí)時(shí)性是非常重要的因素。如果出現(xiàn)滯后或延遲，會(huì)導(dǎo)致仿真系統(tǒng) 的真實(shí)感下降。而在路徑規(guī)劃這方面，已有的技術(shù)專注于系統(tǒng)運(yùn)行期間的一些優(yōu)化技術(shù)，而 未關(guān)注前期的數(shù)據(jù)預(yù)處理。

發(fā)明內(nèi)容
本發(fā)明目的是針對(duì)目前仿真系統(tǒng)的路徑規(guī)劃時(shí)間過長(zhǎng)，提出一種基于矢量法的路 徑規(guī)劃預(yù)處理方法，該方法可以有效提高仿真系統(tǒng)路徑規(guī)劃時(shí)的實(shí)時(shí)處理速度。本發(fā)明的思路是針對(duì)仿真環(huán)境中的移動(dòng)主體不能夠穿越距離小于自身的障礙物 這一事實(shí)，根據(jù)路徑規(guī)劃中最大移動(dòng)主體的尺寸設(shè)置一個(gè)閾值，將距離小于該閾值的障礙 物進(jìn)行合并，從而降低仿真系統(tǒng)中后續(xù)路徑規(guī)劃的工作量，提高整個(gè)系統(tǒng)的實(shí)時(shí)處理能力。具體而言，本發(fā)明采用以下技術(shù)方案
一種基于矢量法的路徑規(guī)劃預(yù)處理方法，用于矢量法表示的仿真系統(tǒng)路徑規(guī)劃，該方 法包括以下步驟
4步驟A、將仿真環(huán)境中的每個(gè)障礙物均模型化為凸多邊形；
步驟B、將步驟A中得到的凸多邊形中距離小于預(yù)先設(shè)定的閾值的任意兩個(gè)凸多邊形 融合處理為一個(gè)新的凸多邊形，并依次迭代處理，直到最終得到的凸多邊形中任意兩個(gè)的 距離均大于上述預(yù)先設(shè)定的閾值；其中，所述預(yù)先設(shè)定的閾值等于路徑規(guī)劃中最大移動(dòng)主 體的尺寸；
步驟C、將步驟B得到的結(jié)果作為仿真系統(tǒng)中路徑規(guī)劃的初始狀態(tài)。仿真系統(tǒng)中的建模方法有很多，如三維建模、柵格法等，為了簡(jiǎn)化計(jì)算并便于后續(xù) 的相交判斷和融合處理，本發(fā)明在將障礙物模型化為凸多邊形時(shí)優(yōu)選在歐式平面中實(shí)現(xiàn)。其中，凸多邊形之間的距離采用最小距離法定義，即兩個(gè)凸多邊形上距離最小的 兩個(gè)點(diǎn)之間的距離。步驟B具體包括以下各步驟
步驟Bi、設(shè)置變量的初始值為步驟A得到的凸多邊形的數(shù)量；
步驟B2、設(shè)置變量、的值為
m=1，n = 1 ；
步驟B3、計(jì)算第n個(gè)凸多邊形與第n + 1個(gè)凸多邊形的距離并判斷該距離是否大于一 個(gè)預(yù)先設(shè)定的閾值，如是，則轉(zhuǎn)步驟B5 ；如否，則轉(zhuǎn)步驟B4 ；
步驟B4、將第n個(gè)凸多邊形與第n+1個(gè)凸多邊形融合為一個(gè)新的凸多邊形并刪除原 第個(gè)與第m +1個(gè)凸多邊形,將況的值修改為N-1后轉(zhuǎn)步驟B2 ；
步驟B5、判斷是否m= N,如是，則轉(zhuǎn)步驟B6;如否，則將的值修改為m + 1后轉(zhuǎn)步驟
B3 ；
步驟B6、判斷是否n=p,如是，則結(jié)束；如否，則將m的值修改為n并將n的值修改為n+1 并后轉(zhuǎn)步驟Β3。本發(fā)明中，將兩個(gè)凸多邊形融合處理為一個(gè)新的凸多邊形，具體按照以下方法
以其中一個(gè)多邊形的中心為原點(diǎn)，該多邊形的兩個(gè)頂點(diǎn)的連線為縱軸，兩個(gè)多邊形的 中心連線為橫軸，建立相對(duì)坐標(biāo)系；在該相對(duì)坐標(biāo)系中，在兩個(gè)多邊形中尋找進(jìn)行縱坐標(biāo)絕 對(duì)值最大的4個(gè)頂點(diǎn)作為一級(jí)融合點(diǎn)；如果一級(jí)融合點(diǎn)的連線與多邊形相交，則在一級(jí)融 合點(diǎn)集合中尋找二級(jí)融合點(diǎn)，使之連線與多邊形不相交，同時(shí)刪除該二級(jí)融合點(diǎn)取代的一 級(jí)融合點(diǎn)；將現(xiàn)有融合點(diǎn)連在一起合成一個(gè)凸多邊形，完成兩個(gè)多邊形的融合。本發(fā)明基于常用的路徑規(guī)劃空間矢量表示法，首先將仿真環(huán)境中的障礙物模型化 為歐氏平面中的凸多邊形；然后對(duì)距離小于某一閾值的任意兩個(gè)凸多邊形融合處理為一個(gè) 新的凸多邊形，并依次迭代處理，直到最終得到的凸多邊形中任意兩個(gè)的距離均大于上述 預(yù)先設(shè)定的閾值。即將距離小于閾值的多障礙物合成一個(gè)，從而有效降低路徑規(guī)劃的計(jì)算 量。相比現(xiàn)有技術(shù)，本發(fā)明的優(yōu)點(diǎn)在于針對(duì)仿真環(huán)境中的移動(dòng)主體不能夠穿越距離小于自 身的障礙物這一事實(shí)，將距離小于某一閾值的障礙物進(jìn)行合并，對(duì)路徑規(guī)劃進(jìn)行預(yù)處理，從 而有效加快實(shí)際的路徑規(guī)劃時(shí)間，提高系統(tǒng)的實(shí)時(shí)處理速度。


圖1是本發(fā)明的基于矢量法的路徑規(guī)劃預(yù)處理方法流程圖； 圖2是本發(fā)明方法中兩個(gè)凸多邊形之間距離計(jì)算的示意圖； 圖3是本發(fā)明方法中點(diǎn)到圖形的距離計(jì)算示意圖，其中(a)為點(diǎn)到多邊形的距離計(jì) 算，(b)為點(diǎn)到直線段的距離計(jì)算；
圖4是本發(fā)明方法中融合點(diǎn)的計(jì)算示意圖，其中(a)為計(jì)算過程，(b)為計(jì)算結(jié)果； 圖5是具體實(shí)施方式
中所述射線與多邊形的相交判斷方法示意圖； 圖6是本發(fā)明方法在融合處理過程中不同階段的示意圖； 圖7是分別采用柵格法和本發(fā)明方法進(jìn)行路徑規(guī)劃的結(jié)果比較； 圖8是分別采用柵格法和本發(fā)明方法進(jìn)行路徑規(guī)劃的時(shí)間結(jié)果比較。
具體實(shí)施例方式下面結(jié)合附圖及實(shí)施例對(duì)本發(fā)明的技術(shù)方案進(jìn)行詳細(xì)說(shuō)明
本實(shí)施例采用如圖6 (a)所示的二維虛擬仿真環(huán)境，經(jīng)模型化處理后共有26個(gè)用凸多 邊形表示的障礙物，根據(jù)該仿真環(huán)境中移動(dòng)主體的尺寸將閾值胃設(shè)定為5。然后按照以下方
法對(duì)這26個(gè)凸多邊形進(jìn)行處理(處理流程如附圖1所示)
步驟Bi、設(shè)置變量—的初始值為步驟A得到的凸多邊形的數(shù)量，本實(shí)施例中y的初始
值即為26;
步驟B2、設(shè)置變量_、■的值為= = 1 ；
步驟Β3、計(jì)算第^個(gè)凸多邊形與第· + 1個(gè)凸多邊形的距離并判斷該距離是否大于一個(gè)
預(yù)先設(shè)定的閾值，如是，則轉(zhuǎn)步驟Β5 ；如否，則轉(zhuǎn)步驟Β4 ；
步驟Β4、將第_個(gè)凸多邊形與第個(gè)凸多邊形融合為一個(gè)新的凸多邊形并刪除原第
■個(gè)與第· + 1個(gè)凸多邊形,將y的值修改為f-Ι后轉(zhuǎn)步驟Β2 ；
步驟B5、判斷是否_% _ I,如是，則轉(zhuǎn)步驟B6 ；如否，則將■的值修改為^ + 1后轉(zhuǎn)步驟
B3 ；
步驟B6、判斷是否^ 況’如是，則結(jié)束；如否，則將■的值修改為■并將■的值修改為 ,| + 1并后轉(zhuǎn)步驟83。本發(fā)明中，任意兩個(gè)多邊形的距離采用最小間距法進(jìn)行計(jì)算并根據(jù)預(yù)先設(shè)定的閾 值判斷是否進(jìn)行融合，
采用最小間距判斷法計(jì)算兩個(gè)多邊形之間的距離，首先如附圖2所示，建立以0
為原心，0込方向?yàn)闄M坐標(biāo)軸方向的相對(duì)坐標(biāo)系"X/，在該坐標(biāo)系下(Jj Jj)
(I = !,2 廣-M)為=(gj 為‘ )(j = 12" - ,Λ)，O1= (0，0)，O2: 0)，I12=先計(jì)算0的所有頂點(diǎn)距離O2的最小值
Siep ι 比較,的橫坐標(biāo)，得到頂點(diǎn)且;=max(i1j, -!ia);
6磊聊2令盡=‘，計(jì)算頂點(diǎn)虞山、蟊j、虞山與多邊形02的距離，分別為‘、乙、毛；罩鋤3若嘲乙‘；毛Ui，則羞土蜃；1，‘毛，轉(zhuǎn)蹋磡4。若鹹‘山；如u當(dāng)，則與乙，轉(zhuǎn)月瓣8。若嘲乙乙，毛)山，則氈一謄斗]，與，當(dāng)，轉(zhuǎn)罩霉咖6；墘頜4計(jì)算頂點(diǎn)躇山與多邊形0，的距離毛；聶聊5若，士晶，則蜃土矗二!，‘‘，轉(zhuǎn)聶鋤4。若，立正，則轉(zhuǎn)聶鋤8；髒咖6計(jì)算頂點(diǎn)舔山與多邊形0，的距離毛；驟蟧7若2女晶，則出生女子!，‘’厶，轉(zhuǎn)墘咖6。若毛逗點(diǎn)，則轉(zhuǎn)墘咖8；雕蟧8輸出0、的所有頂點(diǎn)距離0，的最小值‘。[OO、6] 再計(jì)算02的所有頂點(diǎn)距離o、的最小值 舀鋤l比較軋的橫坐標(biāo)，得到頂點(diǎn)屯，且乙三嘲氈√蟧爭(zhēng)；；；；乩)； 驪中2令呂=默，計(jì)算頂點(diǎn)軋十bo、軋毒l與多邊形0、的距離，分別為‘、乙、毛； 黝3若鹹‘j‘。毛齲，嶼三譬二!，毛‘，轉(zhuǎn)酶4。若鹹‘，‘?！甎沙毛乙，轉(zhuǎn)月鋤8。若鹹小‘i毛)山，則氈三女子]，山乙，轉(zhuǎn)轟啣6；鑼蟧4計(jì)算頂點(diǎn)軋二l與多邊形0、的距離‘；黝5若乩<山，燦土占二l，山‘，轉(zhuǎn)鞠4。若；止山，則轉(zhuǎn)黝8；雕咖6計(jì)算頂點(diǎn)軋毒l與多邊形0、的距離毛；黝7若Z如山，鵬士抖1，毛’毛，轉(zhuǎn)鞠6。若厶乏山，則轉(zhuǎn)黝8；驟雛8輸出02的所有頂點(diǎn)距離0，的最小值厶。盛‘瘤
以上計(jì)算多邊形之間距離的過程中需要求解點(diǎn)到多邊形的距離，本具體實(shí)施方式
中采用以下方法如圖3‘a(chǎn)，，0、昂一躥少女；三’檀山是平面Oxy內(nèi)一個(gè)多邊形，矽為該平面內(nèi)任一點(diǎn)。以矽點(diǎn)為原點(diǎn)，以。、矽連線為橫坐標(biāo)軸，建立相對(duì)坐標(biāo)系?！疛’／’；在該坐標(biāo)系下嗎一‘氣j的，‘r爭(zhēng)L乙 !i腳，，仁‘。，。，，Ol=‘j12，。，，／12----鷗卣]，矽到。、的最近距離‘可計(jì)算如下 磊聊l比較嗎的橫坐標(biāo)，得到頂點(diǎn)翠，，且‘苧砵崠蟧湳!，’‘戴乙3； 舀聊2令手=z，計(jì)算點(diǎn)矽與線段氣／j的距離毛，點(diǎn)矽與線段盡j氣缸的距離乙； 驟蟧3若毛√當(dāng)，則女士聾二1，L’毛，轉(zhuǎn)驟蟧4；若，√五，則女魯云一!，L’毛，轉(zhuǎn)鱷咖6；驟咖4計(jì)算點(diǎn)矽與線段軋扇吼的距離九；5若則轉(zhuǎn)沒—5。若泰M ,則轉(zhuǎn)沒—8 ；
6計(jì)算點(diǎn)Q與線段吣吣丨丨的距離I2
7:若4<￡,則5:=^+1丄=4,轉(zhuǎn)沒印5。若^則轉(zhuǎn)沒響8 ；
8輸出點(diǎn)々與多邊形0的距離/,。
以上計(jì)算多邊形之間距離的過程中需要求解點(diǎn)到直線段的最近距離，本具體實(shí)施 方式中采用以下方法如圖3 (b) ,P1P2為一位于平面fly內(nèi)的直線段，々為該平面內(nèi)任一點(diǎn)。 以該線段的任一端點(diǎn)為原點(diǎn)(圖中選擇的是弋點(diǎn))，以線段所在直線為橫坐標(biāo)軸，建立相對(duì) 坐標(biāo)系，顯然，Λ點(diǎn)和O，點(diǎn)重合。設(shè)々點(diǎn)在下的坐標(biāo)值為(W1),Λ點(diǎn)在 O'X y下的坐標(biāo)值為(^，(^，則以到/^/^的最近距離丄可計(jì)算如下 (1)當(dāng)& < 0 時(shí),￡ =
⑵ ^i>yim,L=d(QsP2)；
(3)當(dāng)丨3511彡12時(shí),￡:計(jì)算0的所有頂點(diǎn)距離O2的最小值以及點(diǎn)與多邊形O2的距離時(shí)，都是從最有可 能產(chǎn)生最小距離的頂點(diǎn)開始，然后往兩邊擴(kuò)展，直到遇到距離大于該點(diǎn)的情況，此時(shí)的最小 距離就是所要求的值。將兩個(gè)凸多邊形融合處理為一個(gè)新的凸多邊形，具體按照以下方法
Step 1 如圖2所示建立的坐標(biāo)系中，尋找兩個(gè)多邊形中縱坐標(biāo)絕對(duì)值最大的4個(gè)頂
點(diǎn)，將這4個(gè)頂點(diǎn)作為一級(jí)融合點(diǎn)，見圖4(a)中的A、A、K3和& ；
Step 2 運(yùn)用直線與多邊形相交算法，對(duì)直線AA和多邊形α進(jìn)行計(jì)算，判斷二者是否
相交Or1點(diǎn)除外)，若相交，求出二級(jí)融合點(diǎn)來(lái)代替A點(diǎn)； Step 3 對(duì)直線AA和多邊形o2 'KliStep 2操作
Step 4 對(duì)直線AA、多邊形O1和多邊形O2重復(fù)總φ 23，直到^vr2與O1和O2
不再相交為止；
5 對(duì)直線&r4、多邊形和多邊形O2重復(fù)2^Up 3和S⑩4 ；
SHAPE \* MERGEF0RMAT陶6 按圖形的順時(shí)針方向，設(shè)定多邊形的融合起點(diǎn)和融
合終點(diǎn)，保留兩點(diǎn)之間的頂點(diǎn)，合成為一個(gè)新多邊形，并且刪除原有多邊形。在圖4(a)中， 多邊形0的融合起點(diǎn)為&，融合終點(diǎn)是&，則按照順時(shí)針方向，保留從&到A的0的頂點(diǎn)； 多邊形込的融合起點(diǎn)為A融合終點(diǎn)是&，同樣按照順時(shí)針方向，保留從^到&的込的頂點(diǎn)。 按照順時(shí)針方向?qū)⒈Ａ舻?和込的頂點(diǎn)連接起來(lái)，得到了新的多邊形久如圖4 (b)所示。在上述求解融合點(diǎn)的計(jì)算步驟中，需要進(jìn)行直線、射線、線段與多邊形相交的判 斷，相應(yīng)的判斷方法如下
(1)直線與多邊形相交判斷算法在相對(duì)坐標(biāo)系中，如果多邊形的各頂點(diǎn)A、B、C、D、E、 F的縱坐標(biāo)值都是同號(hào)，即都是正值或都是負(fù)值，則直線與該多邊形不相交；如果存在異號(hào) 情況，則相交。
(2)射線與多邊形相交判斷算法如圖5所示，P1P2為一條位于平面內(nèi)的射 線，P1為端點(diǎn)，建立如圖中所示的相對(duì)坐標(biāo)系，Z1J2為該平面內(nèi)一多邊形的兩個(gè)位 于橫坐標(biāo)軸兩側(cè)的點(diǎn)，&為Z1么與橫軸的交點(diǎn)，石的橫坐標(biāo)值為x3。并且，弋點(diǎn)不在該多邊 形內(nèi)，則該多邊形與射線ΛΛ的位置關(guān)系可判斷如下
=1 \* GB3①當(dāng)^ < O時(shí)，兩者不相交；
=2 \* GB3②mU3 > O時(shí)，兩者相交。(3)線段與多邊形相交判斷算法分別以線段的兩個(gè)端點(diǎn)為起點(diǎn)，按照線段的方 向建立兩條射線，然后對(duì)這兩條射線與多邊形進(jìn)行相交判斷，與這兩條射線都相交的多邊 形與該線段相交。附圖6顯示了這個(gè)融合實(shí)例的結(jié)果，整個(gè)融合過程一共進(jìn)行了 12步，這里僅顯示 了其中4個(gè)步驟的結(jié)果。融合后，原來(lái)的障礙物用淺顏色線條的多邊形來(lái)表示，說(shuō)明該障礙 物已經(jīng)不存在了，其外圍的深顏色多邊形表示融合后的障礙物。融合結(jié)束后，障礙物之間的 距離都大于5，障礙物的數(shù)量變成了 12個(gè)。將融合結(jié)束后的結(jié)果作為仿真系統(tǒng)中路徑規(guī)劃的初始狀態(tài)，此時(shí)即可采用現(xiàn)有的 各種基于矢量法的方法進(jìn)行路徑規(guī)劃。為了驗(yàn)證本發(fā)明的預(yù)處理方法的效果，采用以下方法進(jìn)行進(jìn)行驗(yàn)證經(jīng)過本發(fā)明 進(jìn)行預(yù)處理之后的仿真環(huán)境采用遺傳算法進(jìn)行路徑規(guī)劃，并和常用的A*算法進(jìn)行比較。針 對(duì)圖7所示的仿真環(huán)境進(jìn)行路徑規(guī)劃比較。圖7顯示了本發(fā)明和A*算法進(jìn)行路徑規(guī)劃的 結(jié)果比較，其中胃的值代表不同尺寸的移動(dòng)主體?？梢钥闯霰景l(fā)明所得的規(guī)劃路徑與A*算
法所得的路徑大致是相同的。圖8顯示了本發(fā)明和A*算法在路徑規(guī)劃時(shí)間上的比較。其中，^是規(guī)劃空間生成 的時(shí)間，對(duì)于A*算法就是規(guī)劃空間的柵格化過程，對(duì)于本發(fā)明就是對(duì)障礙物進(jìn)行融合預(yù)處 理所花費(fèi)的時(shí)間。^是算法搜索路徑的時(shí)間，ι是最終生成的路徑長(zhǎng)度。的單位是秒。 根據(jù)圖8所示的實(shí)驗(yàn)結(jié)果，可以得到以下結(jié)論
(1)當(dāng)移動(dòng)主體外形尺寸較小時(shí)，規(guī)劃空間中的柵格數(shù)量較多，空間柵格化以及A*算 法搜索過程花費(fèi)時(shí)間都較長(zhǎng)。而本發(fā)明算法此時(shí)沒有需要融合的障礙物，也就不用運(yùn)行GA 算法，因此路徑搜索過程非常快，特別是當(dāng)冒=2時(shí)的路徑搜索時(shí)間甚至不到1毫秒，說(shuō)明 了該算法的規(guī)劃速度較快；
(2)當(dāng)移動(dòng)主體外形尺寸逐漸變大時(shí)，規(guī)劃空間中的柵格數(shù)量隨之減少，空間柵格化 以及A*算法搜索過程花費(fèi)時(shí)間都相應(yīng)得迅速減少。而本發(fā)明算法中由于出現(xiàn)了需要融合 的障礙物，搜索時(shí)間增加，導(dǎo)致本發(fā)明算法的路徑搜索過程所花費(fèi)時(shí)間(ff也隨之增加；
(3)隨著移動(dòng)主體外形尺寸的增加，兩種算法規(guī)劃得到的路徑長(zhǎng)度都在增加，但是本 發(fā)明算法得到的路徑長(zhǎng)度絕大多數(shù)都比A*算法的小。
權(quán)利要求
一種基于矢量法的路徑規(guī)劃預(yù)處理方法，用于矢量法表示的仿真系統(tǒng)路徑規(guī)劃，其特征在于，包括以下步驟步驟A、將仿真環(huán)境中的每個(gè)障礙物均模型化為凸多邊形；步驟B、將步驟A中得到的凸多邊形中距離小于預(yù)先設(shè)定的閾值的任意兩個(gè)凸多邊形融合處理為一個(gè)新的凸多邊形，并依次迭代處理，直到最終得到的凸多邊形中任意兩個(gè)的距離均大于上述預(yù)先設(shè)定的閾值；其中，所述預(yù)先設(shè)定的閾值等于路徑規(guī)劃中最大移動(dòng)主體的尺寸；步驟C、將步驟B得到的結(jié)果作為仿真系統(tǒng)中路徑規(guī)劃的初始狀態(tài)。
2.如權(quán)利要求1所述基于矢量法的路徑規(guī)劃預(yù)處理方法，其特征在于，步驟A中所述將 障礙物模型化為凸多邊形是在歐式平面中實(shí)現(xiàn)。
3.如權(quán)利要求1所述基于矢量法的路徑規(guī)劃預(yù)處理方法，其特征在于，步驟B中所述凸 多邊形之間的距離采用最小間距判斷法計(jì)算。
4.如權(quán)利要求1所述基于矢量法的路徑規(guī)劃預(yù)處理方法，其特征在于，所述步驟B具體 包括以下各步驟步驟B1、設(shè)置變量F的初始值為步驟A得到的凸多邊形的數(shù)量；步驟B2、設(shè)置變量的值為= Ij = 1;步驟B3、計(jì)算第廁個(gè)凸多邊形與第個(gè)凸多邊形的距離并判斷該距離是否大于一個(gè) 預(yù)先設(shè)定的閾值，如是，則轉(zhuǎn)步驟B5 ；如否，則轉(zhuǎn)步驟B4 ；步驟B4、將第灣個(gè)凸多邊形與第個(gè)凸多邊形融合為一個(gè)新的凸多邊形并刪除原第 ■個(gè)與第ffl+1個(gè)凸多邊形,將AT的值修改為后轉(zhuǎn)步驟B2 ；步驟B5、判斷是否潔=f,如是，則轉(zhuǎn)步驟B6 ；如否，則將fff的值修改為膽+ 1后轉(zhuǎn)步驟B3 ；步驟B6、判斷是否》= iY,如是，則結(jié)束；如否，則將顯的值修改為H并將■的值修改為并后轉(zhuǎn)步驟B3。
5.如權(quán)利要求1至4中任一項(xiàng)所述基于矢量法的路徑規(guī)劃預(yù)處理方法，其特征在于，所述兩個(gè)凸多邊形融合為一個(gè)新的凸多邊形具體按照以下方法以其中一個(gè)多邊形的中心為原點(diǎn)，該多邊形的兩個(gè)頂點(diǎn)的連線為縱軸，兩個(gè)多邊形的 中心連線為橫軸，建立相對(duì)坐標(biāo)系；在該相對(duì)坐標(biāo)系中，在兩個(gè)多邊形中尋找進(jìn)行縱坐標(biāo)絕 對(duì)值最大的4個(gè)頂點(diǎn)作為一級(jí)融合點(diǎn)；如果一級(jí)融合點(diǎn)的連線與多邊形相交，則在一級(jí)融 合點(diǎn)集合中尋找二級(jí)融合點(diǎn)，使之連線與多邊形不相交，同時(shí)刪除該二級(jí)融合點(diǎn)取代的一 級(jí)融合點(diǎn)；將現(xiàn)有融合點(diǎn)連在一起合成一個(gè)凸多邊形，完成兩個(gè)多邊形的融合；所述連線 包括直線、射線、線段。
6.如權(quán)利要求5所述基于矢量法的路徑規(guī)劃預(yù)處理方法，其特征在于，進(jìn)行直線、射 線、線段與凸多邊形相交的判斷時(shí)分別采用以下判斷方法直線與多邊形相交判斷方法在相對(duì)坐標(biāo)系中，如果多邊形的各頂點(diǎn)的縱坐標(biāo)值都是 同號(hào)，則直線與該多邊形不相交；如果存在異號(hào)情況，則相交；射線與多邊形相交判斷方法以該射線的頂點(diǎn)作為原點(diǎn)，以該射線作為橫軸，建立相對(duì)坐標(biāo)系，多邊形的邊與相對(duì)坐標(biāo)系的交點(diǎn)的橫坐標(biāo)，如果小于0，則該多邊形與射線不相交； 如果大于0，該多邊形與射線相交；線段與多邊形相交判斷方法分別以線段的兩個(gè)端點(diǎn)為起點(diǎn)，按照線段的方向建立兩 條射線，然后對(duì)這兩條射線與多邊形進(jìn)行相交判斷，與這兩條射線都相交的多邊形與該線 段相交。
全文摘要
本發(fā)明公開了一種基于矢量法的路徑規(guī)劃預(yù)處理方法，用于矢量法表示的仿真系統(tǒng)路徑規(guī)劃。本發(fā)明方法首先將仿真環(huán)境中的每個(gè)障礙物均模型化為凸多邊形；然后將凸多邊形中距離小于預(yù)先設(shè)定的閾值的任意兩個(gè)凸多邊形融合處理為一個(gè)新的凸多邊形，并依次迭代處理，直到最終得到的凸多邊形中任意兩個(gè)的距離均大于上述預(yù)先設(shè)定的閾值，其中，所述閾值是根據(jù)路徑規(guī)劃中最大移動(dòng)主體的尺寸進(jìn)行設(shè)置；最后將融合的結(jié)果作為仿真系統(tǒng)中路徑規(guī)劃的初始狀態(tài)。本發(fā)明針對(duì)仿真環(huán)境中的移動(dòng)主體不能夠穿越距離小于自身的障礙物這一事實(shí)，將距離小于移動(dòng)主體尺寸的障礙物進(jìn)行合并，從而降低了仿真系統(tǒng)中后續(xù)路徑規(guī)劃的工作量，提高了整個(gè)系統(tǒng)的實(shí)時(shí)處理能力。
文檔編號(hào)G09B29/00GK101996516SQ20101055274
公開日2011年3月30日 申請(qǐng)日期2010年11月22日 優(yōu)先權(quán)日2010年11月22日
發(fā)明者孟憲權(quán), 趙英男 申請(qǐng)人:南京信息工程大學(xué)