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