【智能巡邏車避障規(guī)劃算法研究5300字】

-1-智能巡邏車避障規(guī)劃算法研究綜述目錄TOC\o"1-3"\h\u28241智能巡邏車避障規(guī)劃算法研究綜述 -1-30251.1智能巡邏車運(yùn)動(dòng)學(xué)模型 -1-298631.1.1坐標(biāo)系轉(zhuǎn)化 -1-273471.1.2驅(qū)動(dòng)模型 -2-225761.2智能巡邏車室內(nèi)環(huán)境建模 -4-150161.3基于改進(jìn)D星算法的路徑規(guī)劃 -5-12881.3.1D星算法 -5-154251.3.2改進(jìn)D星算法 -8-205991.4基于改進(jìn)D星算法仿真分析 -9-1.1智能巡邏車運(yùn)動(dòng)學(xué)模型1.1.1坐標(biāo)系轉(zhuǎn)化為了使智能巡邏車能在二維平面內(nèi)自助導(dǎo)航行走，其定位能力是第一位的。巡邏車需要應(yīng)用傳感器來(lái)測(cè)量其自身和障礙物的距離，從而來(lái)調(diào)整自己目前的姿勢(shì)以免碰撞到障礙物。于此，同時(shí)還需要知道自身在整個(gè)工作環(huán)境的坐標(biāo)位置。那么就涉及到兩個(gè)坐標(biāo)，一個(gè)是以巡邏車自身中心為原點(diǎn)的局部坐標(biāo)，來(lái)反映巡邏車與障礙物之間的相對(duì)位置關(guān)系；另一個(gè)是全局坐標(biāo)，來(lái)反映巡邏車自身在整個(gè)工作環(huán)境的位置。在建立全局坐標(biāo)系和局部坐標(biāo)系后，需要能將2個(gè)分布坐標(biāo)系里的分布坐標(biāo)展開(kāi)互相交換。經(jīng)過(guò)分布坐標(biāo)系的互相交換，才可以得到巡邏車和障礙物的準(zhǔn)確坐標(biāo)信息，這是進(jìn)行準(zhǔn)確路徑規(guī)劃的前提，通過(guò)合理的路徑規(guī)劃巡邏車才可能具備自主的導(dǎo)航能力。如圖1.1所示，XWOWYW表示全局坐標(biāo)系，XSOSYS為局部坐標(biāo)系。其中OW是全局坐標(biāo)系的原點(diǎn)，XW表示全局坐標(biāo)系橫坐標(biāo)軸，圖1.1局部坐標(biāo)系與全局坐標(biāo)系示意圖全局的坐標(biāo)轉(zhuǎn)換化為局部的坐標(biāo)如圖1.1所示，先假定原點(diǎn)OW和OS重合，Q點(diǎn)全局坐標(biāo)為Q（1.1）其中xw=lcosθw,（1.2）局部的坐標(biāo)轉(zhuǎn)換化為全局的坐標(biāo)同理先假定原點(diǎn)OW和OS重合，已知Q的局部坐標(biāo)為Qs（1.3）其中xs=lcosθs,（1.4）1.1.2驅(qū)動(dòng)模型本課題如圖1.2所示的智能巡邏車作為研究對(duì)象。其巡邏車底部由一個(gè)從動(dòng)輪和兩個(gè)差速的主動(dòng)輪組成，系統(tǒng)采用差速的驅(qū)動(dòng)方式驅(qū)動(dòng)。差速驅(qū)動(dòng)方式可以方便的實(shí)現(xiàn)巡邏車原地打轉(zhuǎn)調(diào)節(jié)自身姿勢(shì)，左右轉(zhuǎn)以及前進(jìn)后退等功能。為了實(shí)現(xiàn)巡邏車動(dòng)順產(chǎn)，分析其機(jī)械運(yùn)動(dòng)的可實(shí)現(xiàn)性，本課題對(duì)巡邏車運(yùn)動(dòng)方式進(jìn)行分析，建立其差動(dòng)驅(qū)動(dòng)模型。為了實(shí)現(xiàn)巡邏車運(yùn)動(dòng)模型的建立，本課題首先對(duì)相關(guān)條件進(jìn)行假設(shè)。假設(shè)兩驅(qū)動(dòng)輪是剛性的且直徑相同，驅(qū)動(dòng)輪于地面形成點(diǎn)接觸，驅(qū)動(dòng)軸也是剛性的其與運(yùn)動(dòng)方向垂直。圖1.2巡邏車差動(dòng)驅(qū)動(dòng)幾何模型如圖1.2所示建立了全局坐標(biāo)系XWOWYW，局部坐標(biāo)系XSOS假設(shè)在OS點(diǎn)上的運(yùn)動(dòng)速度是Vx’=V1cosφ將式1.5化簡(jiǎn)可得：x’sinφ?y所以t點(diǎn)在全局坐標(biāo)系上的位置為：xt=x對(duì)式1.6進(jìn)行對(duì)時(shí)間的求導(dǎo)可以計(jì)算出兩個(gè)分量的速度，具體如下式所示：xt=x對(duì)式1.7進(jìn)行簡(jiǎn)化，可得：xtsinφφ=（θ1V1=1綜上所述，可以得到t點(diǎn)的速度矩陣為：xtyt得到t點(diǎn)相對(duì)于全局坐標(biāo)系的運(yùn)動(dòng)坐標(biāo)值，可以根據(jù)路徑規(guī)劃得出的相關(guān)最佳路徑結(jié)合激光傳感器所測(cè)的障礙物距離，可以有效的保證巡邏車自主物障礙運(yùn)行。如果得知t點(diǎn)位置就可以方便的求出兩個(gè)主動(dòng)輪的速度。1.2智能巡邏車室內(nèi)環(huán)境建模環(huán)境建模是對(duì)智能巡邏車工作環(huán)境進(jìn)行抽象，從而建立一種機(jī)器語(yǔ)言可以識(shí)別的空間模型。為了適合D*路徑規(guī)劃算法，本課題采用柵格法進(jìn)行智能巡邏車的環(huán)境建模。根據(jù)本課題智能巡邏車，時(shí)間空間尺寸，考慮到巡邏車的自由活動(dòng)安全閾值，來(lái)最終確定柵格法的最小柵格單元，從而將工作空間劃分若干個(gè)柵格。為了提高智能無(wú)人車輛徑規(guī)劃的精度，根據(jù)實(shí)驗(yàn)環(huán)境實(shí)際大小確定柵格的尺寸，可以最優(yōu)化柵格的數(shù)量，提高路徑規(guī)劃準(zhǔn)確性。采用柵格法建立的環(huán)境模型如圖1.3所示，巡邏車實(shí)際的工作空間為二維空間，白色柵格為無(wú)障礙的柵格部件，也被稱之為自由柵格部件；黑色柵格部件為真實(shí)作業(yè)環(huán)境里的阻礙物，也稱之為障礙柵格。圖1.3柵格環(huán)境模型如圖1.3所示是建立二維直角坐標(biāo)系，智能巡邏車工作環(huán)境模型。假設(shè)工作環(huán)境地圖最大橫向尺寸為，縱向最大尺寸為，每個(gè)柵格的大小為，用來(lái)表示每個(gè)柵格在柵格地圖中的位置，建立每個(gè)柵格的唯一性編碼。柵格劃分的數(shù)學(xué)模型可以表示為：（1.13）式中，表示柵格的第行，表示柵格的第列，柵格的全局坐標(biāo)系表示為。柵格的尺寸用單位長(zhǎng)度來(lái)表示，同時(shí)對(duì)柵格環(huán)境，進(jìn)行信息化編碼。其中將障礙柵格定義為1；將自由柵格定義為0，這樣便于計(jì)算機(jī)對(duì)整個(gè)柵格環(huán)境信息進(jìn)行存儲(chǔ)，形成柵格信息模型，建立柵格電子地圖，便于后續(xù)路徑規(guī)劃使用。為了有效提高路徑規(guī)劃的質(zhì)量，本課題采用八方位柵格鄰居運(yùn)動(dòng)方式，進(jìn)行柵格路徑的搜索。具體搜索方向如圖1.4所示。應(yīng)用相關(guān)算法，在八方位搜索模式運(yùn)動(dòng)方式下，進(jìn)行從開(kāi)始節(jié)點(diǎn)到最終點(diǎn)規(guī)劃出最優(yōu)路徑。最終將柵格直角坐標(biāo)系路徑的指引下，機(jī)器人完成移動(dòng)任務(wù)。圖1.4八方位運(yùn)動(dòng)方式圖1.3基于改進(jìn)D星算法的路徑規(guī)劃為了有效改進(jìn)柵格法在分辨率高以及網(wǎng)格數(shù)較多，時(shí)路徑規(guī)劃實(shí)時(shí)性差問(wèn)題。本課題提出一種應(yīng)用JPS-BitPrune跳點(diǎn)優(yōu)化技術(shù)改進(jìn)的D*智能巡邏車路徑規(guī)劃算法，從而實(shí)現(xiàn)智能巡邏車路徑動(dòng)態(tài)規(guī)劃。其關(guān)鍵性技術(shù)為D*算法和JPS-BitPrune跳點(diǎn)優(yōu)化技術(shù)。1.1.1D星算法為了有效改進(jìn)柵格法在分辨率高以及網(wǎng)格數(shù)較多，時(shí)路徑規(guī)劃實(shí)時(shí)性差問(wèn)題。本課題提出一種應(yīng)用JPS-BitPrune跳點(diǎn)優(yōu)化技術(shù)改進(jìn)的D*室內(nèi)服務(wù)機(jī)器人路徑規(guī)劃算法，從而實(shí)現(xiàn)室內(nèi)服務(wù)機(jī)器人路徑動(dòng)態(tài)規(guī)劃。其關(guān)鍵性技術(shù)為D*算法和JPS-BitPrune跳點(diǎn)優(yōu)化技術(shù)。D*算法主要包括兩個(gè)部分，PROCESS?STATE和MODIFY?COST，前者用來(lái)計(jì)算終點(diǎn)到當(dāng)前節(jié)點(diǎn)的最優(yōu)cost，后者用來(lái)修正cost。算法的符號(hào)與含義為：1）表示機(jī)器人的狀態(tài)State；2）表示一個(gè)指向當(dāng)前狀態(tài)指向父狀態(tài)的指針；3）表示從X到Y(jié)的路徑開(kāi)銷；4)基于傳感器數(shù)據(jù)的X到Y(jié)的路徑開(kāi)銷；5)表示當(dāng)前state的狀態(tài)；6）表示代價(jià)函數(shù)估計(jì)，表示當(dāng)前State到G的開(kāi)銷估計(jì)；7）表示該值是優(yōu)先隊(duì)列Openlist中的排序依據(jù)，K值最小的State位于隊(duì)列頭，對(duì)于處于OpenList上的StateX，表示從X被置于Openlist后，X到G的最小代價(jià)H（X）。8）表示所有位于Openlist上的state的最小K值。對(duì)于機(jī)器人路徑開(kāi)銷值，其最小值可以為單位柵格值，最大為障礙物值。具體取值如表1.1所示。表1.1柵格X到Y(jié)的路徑開(kāi)銷水平或垂直運(yùn)動(dòng)對(duì)角線運(yùn)動(dòng)自由柵格自由柵格障礙柵格障礙柵格D*算法偽代碼為：X=MIN-STATE()IfX=NULLthenreturn-1kold=GET-KMIN():DELETE(X)ifkold<h(X)thenforeachneighborYofX:ifh(Y)koldandh(X)>h(Y)+c(X,Y)then b(X)=Y;h(X)=h(Y)+c(X,Y)ifkold=h(X)thenforeachneighborYofX:ift(Y)=NEWor (b(Y)=Xandh(Y)h(X)+c(XY))or (b(Y)Xandh(Y)>h(X)+c(X,Y))then b(Y)=X;INSERT(Y,h(X)+c(X,Y))elseforeachneighborYofX:ift(Y)=NEWor (b(Y)=Xandh(Y)h(X)+c(XY))then b(Y)=X;INSERT(Y,h(X)+c(X,Y))else ifb(Y)Xandh(Y)>h(X)+c(XY)then INSERT(X,h(X)) else ifb(Y)t(X)andh(Y)>h(X)+c(XY)and t(X)=CLOSEDandh(Y)>koldthen INSERT(Y,h(Y))ReturnGET-KMIN()1.當(dāng)X處于Lower狀態(tài)時(shí)，即h(Y)≤kold代碼中首先將openlist中最低K值的X移除，X的所有鄰接state都被檢測(cè)是否其路徑代價(jià)可以更低，狀態(tài)為New的鄰接state被賦予初始路徑開(kāi)銷值，并且開(kāi)銷的變動(dòng)被傳播給每一個(gè)backpointer指向X的鄰接stateY（不管這個(gè)新的開(kāi)銷比原開(kāi)銷大或者小），也就是說(shuō)只要你指向了X，那么X的路徑開(kāi)銷變動(dòng)時(shí)，你的路徑代價(jià)必須隨之改變。這里可能存在由于X路徑開(kāi)銷變動(dòng)過(guò)大，Y可以通過(guò)非X的其他state到達(dá)目標(biāo)且路徑開(kāi)銷更小的情況，這點(diǎn)在L8-13中并沒(méi)有處理，而是放在后續(xù)針對(duì)Y的process-state函數(shù)中，在對(duì)Y進(jìn)行處理時(shí)，會(huì)將其backpointer指向周圍路徑開(kāi)銷最小的state。如果X的鄰接State狀態(tài)為New時(shí)，應(yīng)將其鄰接state的backpointer指向X。所有路徑開(kāi)銷有所變動(dòng)的state都被置于Openlist中進(jìn)行處理，從而將變動(dòng)傳播給鄰接的state。2.當(dāng)X處于Raise狀態(tài)時(shí)，即kold=h(X)說(shuō)明X為狀態(tài)，路徑開(kāi)銷不是最優(yōu)的。那么就要通過(guò)找到h(Y)≤kold最優(yōu)開(kāi)銷節(jié)點(diǎn)，從而優(yōu)化X的路徑開(kāi)銷，后將X的backpointer指向其neighbor。如果存在更短的路徑，則將開(kāi)銷變動(dòng)傳播到狀態(tài)為New的鄰居state。如果X可以使一個(gè)backpointer并不指向X的鄰居state的路徑開(kāi)銷最小，即Y通過(guò)X到目標(biāo)G的距離更短，但是此時(shí)Y的backpointer并不指向X，針對(duì)這種情況，可以將X重新置于Openlist中進(jìn)而優(yōu)化Y。如果X可以通過(guò)一個(gè)狀態(tài)為closed的并不是最理想的鄰居stateY來(lái)減小路徑開(kāi)銷，那么將Y重新置于Openlist中。1.1.2改進(jìn)D星算法為了在高分辨率柵格地圖中，有效提高D*路徑規(guī)劃的實(shí)時(shí)性。本課題提出一種基于JPS-BitPrune動(dòng)態(tài)跳點(diǎn)算法的D*機(jī)器人路徑規(guī)劃算法。JPS又名跳點(diǎn)搜索算法（JumpPointSearch），是由澳大利亞兩位教授于2011年提出的基于Grid格子的尋路算法。而JPS-BitPrune算法支持動(dòng)態(tài)阻擋，當(dāng)動(dòng)態(tài)阻擋出現(xiàn)時(shí)，將格子標(biāo)記為阻擋，當(dāng)動(dòng)態(tài)阻擋消失時(shí)，將格子標(biāo)記為非阻擋，如果動(dòng)態(tài)阻擋占據(jù)k個(gè)格子，則時(shí)間復(fù)雜度為O(k)。JPS-BitPrune算法的特點(diǎn)為，保留D*算法的框架的同時(shí)，進(jìn)一步優(yōu)化了D*算法尋找后繼節(jié)點(diǎn)的操作。JPS和D*算法之間的主要區(qū)別是后續(xù)的節(jié)點(diǎn)擴(kuò)展策略。這與D*算法策略不同，后者直接獲取當(dāng)前節(jié)點(diǎn)的所有未封閉和可到達(dá)的鄰居進(jìn)行擴(kuò)展。JPS基于當(dāng)前節(jié)點(diǎn)的當(dāng)前方向，基于用于擴(kuò)展后續(xù)節(jié)點(diǎn)的搜索跳轉(zhuǎn)點(diǎn)策略，并且遵循“兩個(gè)定義，三個(gè)規(guī)則”。JPS算法基本術(shù)語(yǔ)定義：1）current 表示當(dāng)前節(jié)點(diǎn)2）openset表示開(kāi)啟節(jié)點(diǎn)集合，集合內(nèi)節(jié)點(diǎn)有待進(jìn)一步拓展3）closedset關(guān)閉節(jié)點(diǎn)結(jié)合，集合內(nèi)節(jié)點(diǎn)不再拓展4）neighbor鄰居，與當(dāng)前節(jié)點(diǎn)相鄰的節(jié)點(diǎn)5）parent(x)節(jié)點(diǎn)x的父節(jié)點(diǎn)，即算法尋得的路徑中節(jié)點(diǎn)parent(x)的下一節(jié)點(diǎn)為x表1.2JPS算法的“兩個(gè)定義、三個(gè)規(guī)則”定義一，強(qiáng)迫鄰居如果節(jié)點(diǎn)n是x的鄰居，并且節(jié)點(diǎn)n的鄰居有阻擋（不可行走的格子），并且從parent(x)、x、n的路徑長(zhǎng)度比其他任何從parent(x)到n且不經(jīng)過(guò)x的路徑短，其中parent(x)為路徑中x的前一個(gè)點(diǎn)，則n為x的強(qiáng)迫鄰居，x為n的跳點(diǎn)。定義二，跳點(diǎn)1）如果點(diǎn)y是起點(diǎn)或目標(biāo)點(diǎn)，則y是跳點(diǎn)；2）如果y有強(qiáng)迫鄰居則y是跳點(diǎn)；3）如果parent(y)到y(tǒng)是對(duì)角線移動(dòng)，并且y經(jīng)過(guò)水平或垂直方向移動(dòng)可以到達(dá)跳點(diǎn)，則y是跳點(diǎn)。規(guī)則一JPS搜索跳點(diǎn)的過(guò)程中，如果直線方向和對(duì)角線方向都可以移動(dòng)，則首先在直線方向搜索跳點(diǎn)，再在對(duì)角線方向搜索跳點(diǎn)。基本規(guī)則二（1）假如從parent(x)到x是直線機(jī)械移動(dòng)，n是x的鄰居，如果有從parent(x)到n的途徑不通過(guò)x并且途徑有效長(zhǎng)度低于或等同于從parent(x)通過(guò)x到n的途徑，則走到x后下一個(gè)點(diǎn)不會(huì)走到n；（2）假如從parent(x)到x是針對(duì)角線機(jī)械移動(dòng)，n是x的鄰居，如果有從parent(x)到n的途徑不通過(guò)x并且途徑有效長(zhǎng)度低于從parent(x)通過(guò)x到n的途徑，則走到x后下一個(gè)點(diǎn)不會(huì)走到n。規(guī)則三只有跳點(diǎn)才會(huì)加入openset，因?yàn)樘c(diǎn)會(huì)改變行走方向，而非跳點(diǎn)不會(huì)改變行走方向，最后尋找出來(lái)的路徑點(diǎn)也都是跳點(diǎn)。1.4基于改進(jìn)D星算法仿真分析如圖1.5所示，5*5的網(wǎng)格，黑色代表阻擋區(qū)，S為起點(diǎn)，E為終點(diǎn)。JPS要尋找從S到E的最短路徑，首先初始化將起點(diǎn)S加入openset。從openset取出F值最小的點(diǎn)S，并從openset刪除，加入closedset，S的當(dāng)前方向?yàn)榭眨瑒t沿八個(gè)方向?qū)ふ姨c(diǎn)，在該圖中從S出發(fā)只有下、右、右下圖1.5JPS尋路示意圖三個(gè)方向可走，但向下搜索到D遇到邊界，向右搜索到F遇到阻擋，因此都沒(méi)有找到跳點(diǎn)，然后沿右下方向?qū)ふ姨c(diǎn)，在G點(diǎn)，根據(jù)上文定義二的第（3）條，parent(G)為S，praent(G)到S為對(duì)角線移動(dòng)，并且G經(jīng)過(guò)垂直方向移動(dòng)可以到達(dá)跳點(diǎn)I，因此G為跳點(diǎn)，將G加入openset。從openset取出F值最小的點(diǎn)G，并從openset刪除，加入closedset，因?yàn)镚當(dāng)前方向?yàn)閷?duì)角線方向（從S到G的方向），因此在右、下、右下三個(gè)方向?qū)ふ姨c(diǎn)，在該圖中從G出發(fā)只有向下可走，因此向下尋找跳點(diǎn)，根據(jù)上文定義二的第（2）條找到跳點(diǎn)I，將I加入openset。從openset取出F值最小的點(diǎn)I，并從openset刪除，加入closedset，因?yàn)镮的當(dāng)前方向?yàn)橹本€方向（從G到I的方向），在I點(diǎn)時(shí)上方不可走且右方、前方可走，因此沿下、右、右下找跳點(diǎn)，但右、下都遇到邊界，只有向右尋找到跳點(diǎn)Q，因此將Q加入openset。從openset取出F值最小的點(diǎn)Q，并從openset刪除，加入closedset，因?yàn)镼的當(dāng)前方向?yàn)橹本€方向，Q的左方不可走且上方、下方可走，因此沿上、下、右下尋找跳點(diǎn)，但上、下都遇到邊界，只有向上尋找到跳點(diǎn)E，因此將E加入openset。從openset取出F值最小的點(diǎn)E，因?yàn)镋是目標(biāo)點(diǎn)，因此尋路結(jié)束，路徑是S、G、I、Q、E。本課題不考慮從H能走到K的情況，因?yàn)閷?duì)角線有阻擋。上述的JPS尋路效率是明顯快于D*的，原因在于：在從S到A沿垂直方向?qū)ぢ窌r(shí)，在A點(diǎn)，如果是D*