一種基于邊界值問(wèn)題的滾動(dòng)航路規(guī)劃方法
【專(zhuān)利摘要】本發(fā)明公開(kāi)了一種基于邊界值問(wèn)題的滾動(dòng)航路規(guī)劃方法，屬于航路規(guī)劃領(lǐng)域，本發(fā)明首先根據(jù)進(jìn)行地形網(wǎng)格建模，之后采用滾動(dòng)規(guī)劃策略，將全局最優(yōu)近似分解為每個(gè)時(shí)域窗口的局部最優(yōu)，在每個(gè)時(shí)域窗口內(nèi)求解邊界值問(wèn)題得到局部最優(yōu)解。利用直線(xiàn)-視線(xiàn)方法設(shè)計(jì)時(shí)域窗口的子目標(biāo)，并在滾動(dòng)窗口內(nèi)完成了勢(shì)場(chǎng)更新與航路計(jì)算。本發(fā)明借鑒了滾動(dòng)規(guī)劃的思想，通過(guò)求解離散條件下的邊界值問(wèn)題，能夠?qū)崟r(shí)跟蹤運(yùn)動(dòng)目標(biāo)。在滿(mǎn)足局部最優(yōu)性的前提下，對(duì)全局最優(yōu)進(jìn)行近似，最終完成跟蹤運(yùn)動(dòng)目標(biāo)的航路規(guī)劃任務(wù)。本發(fā)明地形建模簡(jiǎn)單，子目標(biāo)選擇計(jì)算量小，時(shí)域窗口設(shè)計(jì)合理，實(shí)現(xiàn)方便。
【專(zhuān)利說(shuō)明】一種基于邊界值問(wèn)題的滾動(dòng)航路規(guī)劃方法

【技術(shù)領(lǐng)域】
[0001] 本發(fā)明屬于航路規(guī)劃領(lǐng)域，具體地說(shuō)是涉及一種基于邊界值問(wèn)題的滾動(dòng)航路規(guī)劃 方法。

【背景技術(shù)】
[0002] 航路規(guī)劃是影響智能體自主行為的關(guān)鍵技術(shù)，一直受到各方面的高度重視，經(jīng)過(guò) 幾十年的研究和發(fā)展，取得了大量研究成果，為目前智能體的大發(fā)展奠定了基礎(chǔ)。航路規(guī)劃 能力是智能體自主性和智能性的重要標(biāo)志，經(jīng)過(guò)多年的研究與發(fā)展形成了眾多分支，其中 一個(gè)研究熱點(diǎn)就是航路的實(shí)時(shí)性和最優(yōu)性問(wèn)題，這類(lèi)問(wèn)題屬于動(dòng)態(tài)航路規(guī)劃，有的也稱(chēng)為 實(shí)時(shí)規(guī)劃與重規(guī)劃。
[0003] 目前見(jiàn)諸文獻(xiàn)的航路規(guī)劃方法有：基于圖形的規(guī)劃方法、決策型搜索方法、隨機(jī)搜 索方法和人工勢(shì)場(chǎng)法等。以Voronoi圖法和Probabilistic Roadmap Method(PRM)法為代 表的基于圖形的方法存在組合爆炸的問(wèn)題，因此不是很適合跟蹤運(yùn)動(dòng)目標(biāo)的實(shí)時(shí)規(guī)劃。雖 然A*和D*可以通過(guò)算法的改進(jìn)從而改善實(shí)時(shí)性，但是這種改進(jìn)的能力也是有限的，即算法 的結(jié)構(gòu)限制了計(jì)算的效率。它們均有一定的拓?fù)淠芰Γ?guī)劃出的航路具備一定最優(yōu)性，但 是很難滿(mǎn)足實(shí)時(shí)性要求。在隨機(jī)型搜索方法中，通過(guò)重新編碼和改進(jìn)進(jìn)化算子能夠使遺傳 算法能夠滿(mǎn)足實(shí)時(shí)規(guī)劃的要求，但是仍然需要額外的工作來(lái)解決過(guò)早收斂和試湊調(diào)參的問(wèn) 題。許多研究表明，這些問(wèn)題同樣存在于蟻群算法和粒子群算法。這些方法的另一個(gè)共同 特點(diǎn)是需要網(wǎng)格建模來(lái)描述環(huán)境，而網(wǎng)格模型設(shè)計(jì)的不合理會(huì)導(dǎo)致航路曲率較大。
[0004] 勢(shì)場(chǎng)法在航路的平滑性上具有很強(qiáng)的優(yōu)勢(shì)，因?yàn)榇蠖鄶?shù)勢(shì)場(chǎng)法把物體的運(yùn)動(dòng)看成 是作用力的結(jié)果，而作用力通常是連續(xù)的，并不需要地形的網(wǎng)格化，因此避免了航路被離散 成航路點(diǎn)。勢(shì)場(chǎng)法除了航路平滑以外實(shí)時(shí)性也很高，尤其在復(fù)雜地形中表現(xiàn)明顯。模擬退 火算法和電荷法屬于傳統(tǒng)的勢(shì)場(chǎng)法，必須要忍受局部極小，這也是調(diào)和場(chǎng)類(lèi)的方法存在的 共同問(wèn)題。流函數(shù)法借鑒流體力學(xué)的概念建立勢(shì)場(chǎng)區(qū)域，并被證明能夠避免局部極小，通過(guò) 對(duì)單個(gè)障礙的流函數(shù)加權(quán)求和能夠解決障礙物接觸時(shí)的路徑規(guī)劃問(wèn)題，之后該方法被進(jìn)一 步擴(kuò)展到了三維。雖然具有諸多優(yōu)勢(shì)，但由于流體概念的限制，流函數(shù)法存在駐點(diǎn)，可能引 起規(guī)劃終止。
[0005] 航路規(guī)劃方法中經(jīng)常會(huì)遇到實(shí)時(shí)性與最優(yōu)性的沖突問(wèn)題，實(shí)時(shí)性體現(xiàn)了一種動(dòng)態(tài) 的時(shí)變特性，因此每個(gè)時(shí)刻擁有一個(gè)最優(yōu)解，在無(wú)法預(yù)知和對(duì)這種時(shí)變特性建模的情況下， 很難求得patrol最優(yōu)解。如何能夠在存在運(yùn)動(dòng)目標(biāo)的動(dòng)態(tài)情況下，在保證實(shí)時(shí)性的前提下 兼顧最優(yōu)性，達(dá)到實(shí)時(shí)性和最優(yōu)性的折中，是設(shè)計(jì)動(dòng)態(tài)航路規(guī)劃系統(tǒng)時(shí)需要面對(duì)的一個(gè)問(wèn) 題。


【發(fā)明內(nèi)容】

[0006] 針對(duì)現(xiàn)有技術(shù)中存在的問(wèn)題，本發(fā)明提出一種基于邊界值問(wèn)題的滾動(dòng)航路規(guī)劃方 法，通過(guò)該方法能夠在跟蹤運(yùn)動(dòng)目標(biāo)的情況下，快速的生成一條具有局部最優(yōu)特性的航路。
[0007] 本發(fā)明的技術(shù)方案為一種基于邊界值問(wèn)題的滾動(dòng)航路規(guī)劃方法，其首先根據(jù)進(jìn)行 地形網(wǎng)格建模，之后采用滾動(dòng)規(guī)劃策略，將全局最優(yōu)近似分解為每個(gè)時(shí)域窗口的局部最優(yōu)， 在每個(gè)時(shí)域窗口內(nèi)求解邊界值問(wèn)題得到局部最優(yōu)解；再利用直線(xiàn)-視線(xiàn)方法設(shè)計(jì)時(shí)域窗口 的子目標(biāo)，并在滾動(dòng)窗口內(nèi)完成了勢(shì)場(chǎng)更新與航路計(jì)算；具體包括如下步驟：
[0008] 步驟一：根據(jù)不同對(duì)象的物理約束，進(jìn)行陷阱地形預(yù)處理；
[0009] 在航路規(guī)劃前需要根據(jù)使用對(duì)象的轉(zhuǎn)彎半徑&，針對(duì)陷阱區(qū)域進(jìn)行預(yù)處理；如果 智能體無(wú)法在凹字形區(qū)域完成180°轉(zhuǎn)彎，則預(yù)處理后凹字形區(qū)域?qū)⒈惶畛洌凰鲛D(zhuǎn)彎半 徑為智能體的特定物理約束用數(shù)學(xué)公式的描述；
[0010] 步驟二：建立柵格化地形模型，進(jìn)行柵格坐標(biāo)轉(zhuǎn)換；
[0011] 將長(zhǎng)和寬分別為length和width的實(shí)際地形進(jìn)行柵格化，并在計(jì)算機(jī)中存儲(chǔ)為i 行j列的矩陣A ;通過(guò)柵格坐標(biāo)的轉(zhuǎn)換，建立了離散后的網(wǎng)格地形與實(shí)際地形的一一對(duì)應(yīng)關(guān) 系；
[0012] 當(dāng)已知實(shí)際地形中的位置（X，y)，根據(jù)式（1)計(jì)算出該位置所對(duì)應(yīng)的計(jì)算機(jī)存儲(chǔ) 中的元素A(i，j)，
[0013]

【權(quán)利要求】
1. 一種基于邊界值問(wèn)題的滾動(dòng)航路規(guī)劃方法，其首先根據(jù)進(jìn)行地形網(wǎng)格建模，之后采 用滾動(dòng)規(guī)劃策略，將全局最優(yōu)近似分解為每個(gè)時(shí)域窗口的局部最優(yōu)，在每個(gè)時(shí)域窗口內(nèi)求 解邊界值問(wèn)題得到局部最優(yōu)解；再利用直線(xiàn)-視線(xiàn)方法設(shè)計(jì)時(shí)域窗口的子目標(biāo)，并在滾動(dòng) 窗口內(nèi)完成了勢(shì)場(chǎng)更新與航路計(jì)算；具體包括如下步驟： 步驟一：根據(jù)不同對(duì)象的物理約束，進(jìn)行陷阱地形預(yù)處理； 在航路規(guī)劃前需要根據(jù)使用對(duì)象的轉(zhuǎn)彎半徑Ro,針對(duì)陷阱區(qū)域進(jìn)行預(yù)處理；如果智能 體無(wú)法在凹字形區(qū)域完成180°轉(zhuǎn)彎，則預(yù)處理后凹字形區(qū)域?qū)⒈惶畛?；所述轉(zhuǎn)彎半徑為 智能體的特定物理約束用數(shù)學(xué)公式的描述； 步驟二：建立柵格化地形模型，進(jìn)行柵格坐標(biāo)轉(zhuǎn)換； 將長(zhǎng)和寬分別為length和width的實(shí)際地形進(jìn)行柵格化,并在計(jì)算機(jī)中存儲(chǔ)為i行j 列的矩陣A ;通過(guò)柵格坐標(biāo)的轉(zhuǎn)換，建立了離散后的網(wǎng)格地形與實(shí)際地形的一一對(duì)應(yīng)關(guān)系； 當(dāng)已知實(shí)際地形中的位置（x，y)，根據(jù)式（1)計(jì)算出該位置所對(duì)應(yīng)的計(jì)算機(jī)存儲(chǔ)中的 元素 A(i, j)，
當(dāng)已知計(jì)算機(jī)存儲(chǔ)中的元素A(i，j)，根據(jù)式（2)計(jì)算出該元素所對(duì)應(yīng)的實(shí)際地形中的 位置（X，y)，
式中side表示網(wǎng)格單元的邊長(zhǎng)，符號(hào)□表示元素取整操作； 步驟三：確定滾動(dòng)時(shí)域窗口，求解子目標(biāo)點(diǎn)； 3. 1滾動(dòng)時(shí)域窗口的設(shè)計(jì)：子目標(biāo)點(diǎn)在時(shí)域窗口中為一個(gè)位于窗口邊界坐標(biāo)點(diǎn)，并將 時(shí)域窗口做以下兩步處理： 1) 將子目標(biāo)點(diǎn)擴(kuò)展為三個(gè)相鄰的坐標(biāo)點(diǎn)，增加子目標(biāo)點(diǎn)對(duì)智能體的吸引，提高邊界值 勢(shì)場(chǎng)的收斂速度； 2) 將窗口邊界人為設(shè)定成高勢(shì)場(chǎng)，保證邊界對(duì)智能體的排斥作用； 3. 2子目標(biāo)點(diǎn)的計(jì)算： 1) 設(shè)在某個(gè)時(shí)刻tk，對(duì)應(yīng)的時(shí)域窗口為HWk ;在定位智能體當(dāng)前位置和確定HWk大小之 后；時(shí)域窗口一般為全局地圖大小的1/10,從全局地圖中獲得HW k內(nèi)的局部障礙信息； 2) 為當(dāng)前智能體位置，匕,為當(dāng)前目標(biāo)位置，〇。_是與直線(xiàn)相交，并且距 離最近的一個(gè)障礙，V tl、和vbl分別為0。_的四個(gè)頂點(diǎn)； 3) 根據(jù)LOS(Line-Of-Sight)算法，在0。_的四個(gè)頂點(diǎn)中找出阻擋了直線(xiàn)/^的點(diǎn) Vmid ; 4)根據(jù)〇。_和HWk的不同相對(duì)位置，則子目標(biāo)點(diǎn)為直線(xiàn)f 或與HWk的交點(diǎn) 步驟四：求解時(shí)域窗口內(nèi)的邊界值問(wèn)題； 地形環(huán)境已經(jīng)柵格化為矩陣A，每個(gè)柵格A (i，j)存儲(chǔ)著t時(shí)刻的勢(shì)場(chǎng)值it·，柵格單位 的變長(zhǎng)為side ;對(duì)狄氏邊界條件的勢(shì)場(chǎng)進(jìn)行初始化：障礙和邊界所在的柵格勢(shì)場(chǎng)為1，目標(biāo) 點(diǎn)的勢(shì)場(chǎng)為〇 ;其他網(wǎng)格的勢(shì)場(chǎng)根據(jù)不同的地形情況，采用數(shù)值迭代方法求解，其采用GS方 法，首先用式（3)進(jìn)行勢(shì)場(chǎng)更新：
其中 A； = Aj，凡=A+ij，A = /Vi./，Λ = Aj+i，A = A,/-1，v = (vx，vy)，上標(biāo) t 表 示當(dāng)前時(shí)刻，t+1表示下一時(shí)刻。為了確定參數(shù)￥和ε的值域，將式（3)整理為：
其中 wx= eVx/2，wy= eVy/2;當(dāng) wx，wye [1,1]時(shí)上式滿(mǎn)足 pnlin彡pc彡pnlaχ，pnlin和p nlaχ 分別代表與網(wǎng)格內(nèi)的最小和最大勢(shì)場(chǎng)值；設(shè)障礙和目標(biāo)的勢(shì)場(chǎng)值分別為Pmax和Pmin，則任一 網(wǎng)格的梯度下降方向?qū)⒅赶蚰繕?biāo)點(diǎn)并盡量遠(yuǎn)離障礙；定義v是單位向量并且ε e (-2, 2); 步驟五：計(jì)算梯度，將最速下將方向作為前進(jìn)方向，并且根據(jù)八方向法確定航路點(diǎn)； 勢(shì)場(chǎng)更新之后，按照式（5)計(jì)算每個(gè)網(wǎng)格的梯度 ：
設(shè)智能體當(dāng)前所在網(wǎng)格為A(i，j)，前進(jìn)方向?yàn)閂Pi，j;以網(wǎng)格A(i，j)為中心點(diǎn)按照八 方向?qū)ο噜従W(wǎng)格進(jìn)行分割，找出A(i，j)的梯度▽ Pu所指向的區(qū)域，則智能體下一時(shí)刻速 度的期望方向?yàn)楱?PH，j+1，下一時(shí)刻期望到達(dá)的網(wǎng)格為A(i_l，j+Ι); 步驟六：跟蹤運(yùn)動(dòng)目標(biāo)，進(jìn)行滾動(dòng)規(guī)劃，完成整個(gè)航路規(guī)劃過(guò)程； 設(shè)航路規(guī)劃的開(kāi)始時(shí)間為h，根據(jù)步驟三?步驟五的方法，得到、時(shí)刻的HW的航路，然 后將tQ時(shí)刻的HW中子目標(biāo)點(diǎn)作為下一時(shí)刻h的HW中起始點(diǎn)，重復(fù)驟三?步驟五的方法； 通過(guò)將上一時(shí)刻時(shí)域窗口的子目標(biāo)點(diǎn)作為下一時(shí)刻時(shí)域窗口的起點(diǎn)，整個(gè)過(guò)程不斷重復(fù)滾 動(dòng)，智能體最終能夠完成跟蹤運(yùn)動(dòng)目標(biāo)的航路規(guī)劃。
2.根據(jù)權(quán)利要求1所述的一種基于邊界值問(wèn)題的滾動(dòng)航路規(guī)劃方法，其特征在于：所 述步驟四中勢(shì)場(chǎng)更新采用SOR(successive overrelaxation)方法。
【文檔編號(hào)】G01C21/00GK104121903SQ201410317276
【公開(kāi)日】2014年10月29日 申請(qǐng)日期:2014年7月4日 優(yōu)先權(quán)日:2014年7月4日
【發(fā)明者】梁宵, 陳俠, 孟光磊 申請(qǐng)人:沈陽(yáng)航空航天大學(xué)