Field Ｄ*路徑規劃算法
- 1.栅格法路徑規劃存在的問題
- 2.Filed D*算法主要思想解析
- 3.Filed D*算法僞代碼
- 4.算法優化
- 5.算法總結
- 參考文獻
- 搜尋算法其他文章
緊接着上一篇D* Lite路徑規劃算法，這一篇介紹D* Lite算法的改進版Field Ｄ*。
Filed D_star算法是D_star Lite算法的一種改進版本，該算法針對基于栅格的路徑規劃算法通常以栅格端點或中心點作為路徑的節點，限制了路徑方向變化隻能為π/4的倍數，會導緻機器人不必要的運動轉向，影響執行效率。而Dave Ferguson提出的Filed D*算法，通過對栅格進行線性插值使路徑點不局限于端點，規劃方向的變化不再受限于π/4的倍數，生成較為平滑的路徑。
1.栅格法路徑規劃存在的問題
在規劃2D網格時，通常從每個網格單元的中心進行規劃，并且隻允許轉換到相鄰網格單元的中心。這會将限制路徑方向變化隻能為π/4的倍數，進而導緻路徑為次優的，并且在實踐中難以實作。如下圖1所示，在實際的路徑規劃中，規劃的路徑隻能走s點的8個方向（s1,s2,s3,s4,s5,s6,s7,s8），隻能為π/4的倍數。
圖1
舉個例子，機器人在如下圖2沒有障礙物的環境中，需要從起始點s運動到目标點g。顯然，最優化的路徑是起始點s到目标點g的直線。但是機器人的朝向角度為π/8，不是π/4的倍數，因而不能直接走s、g間的直線，隻能按照 e 1 e_1 e1， e 2 e_2 e2走折線段。根據圖中的計算可知，走 e 1 e_1 e1， e 2 e_2 e2的路線約多走8%的路程。
圖2
有一種方式可以緩解這種困難，例如在機器人起點s，尋找路徑時，找到從s到p的直線路徑無碰撞的最遠點p，然後用這條直線路徑替換原來的p路徑（如圖3）。但是，也不是總是有效的。在這裡，基于網格的路徑從s到g(頂部，黑色)不能平滑，因為有四個障礙單元(陰影)。最佳路徑顯示為虛線(藍色)。
圖3
2.Filed D*算法主要思想解析
Filed D*算法的關鍵是在給定相鄰節點路徑代價的前提下，提出一種計算每個網格節點路徑代價的新方法。在基于網格的路徑規劃中，一個節點的代價為（從節點到目标的路徑代價）：
其中，nbrs(s)是s的所有相鄰節點的集合；c(s, s’)是周遊s和s’之間的邊的代價，g(s’)是節點s’的路徑代價。而在此之前都是假設從節點s到相鄰節點的唯一可能的移動隻能是直線軌迹（隻能是從一格移動到另一格）。而Filed D*考慮放寬這個假設，允許從節點s到其網格單元邊界上的任何一點的直線軌迹（如下圖4所示）。如果知道每一個點 s b s_b sb在邊界的值,那麼僅僅可以通過最小化 c ( s , s b ) + g ( s b ) c(s, s_b)+ g(s_b) c(s,sb)+g(sb)計算節點s的最優值， c ( s , s b ) c(s, s_b) c(s,sb)通過s和sb之間的距離乘以到達s所在單元的代價。但是，這樣的點有無數個，是以不可能為每個點計算 g ( s b ) g(s_b) g(sb)。
圖4
但是，用線性插值法對每個邊界點提供g(sb)的近似是可能的。首先将圖4中的節點視為連續代價的樣本點，由節點s開始的一條優化路徑必須通過連接配接兩個連續相鄰的邊緣,例如 s 1 s 2 ⃗ \vec{s_1s_2} s1s2
。是以，設定通過這些邊的路徑的最小代價為s的路徑代價，而這些邊被認為是一次一條。為了通過 s 1 s 2 ⃗ \vec{s_1s_2} s1s2
計算節點s的路徑，需要使用節點 s 1 s_1 s1和 s 2 s_2 s2的路徑代價以及圖5（a）中所示中間網格的轉移代價c和底部網格的轉移代價b。
圖5
假設位于 s 1 s_1 s1和 s 2 s_2 s2之間邊界上的任意點sy的路徑代價是 g ( s 1 ) g(s_1) g(s1)和 g ( s 2 ) g(s_2) g(s2)的線性組合（如圖6）:
圖6
其中，其中y是 s 1 s_1 s1到 s y s_y sy的距離(假設機關單元格)。這個假設并不完美， s y s_y sy的路徑代價可能不是 g ( s 1 ) g(s_1) g(s1)和 g ( s 2 ) g(s_2) g(s2)的線性組合，甚至也不是這些路徑代價的函數。然而，這種線性近似在實際應用中效果良好，可以構造節點s路徑代價的封閉形式解。根據這個近似，s在 s 1 s_1 s1、 s 2 s_2 s2、網格代價c和b下的路徑代價可以計算為
圖7
這個公式可以通過上圖7中的（a）圖了解。公式中的第1項為s→ s x s_x sx的代價，第2項為 s x s_x sx→ s y s_y sy的代價，第3、4項為y處的代價。其中， x ∈ [ 0 , 1 ] x\in[0, 1] x∈[0,1]， y ∈ [ 0 , 1 ] y\in[0, 1] y∈[0,1]。當x=y=1時，路徑為底部的路徑，但是代價卻是中間網格的代價。
令 ( x ⃗ , y ⃗ ) (\vec{x},\vec{y}) (x
,y
)是一對解決上述最小化x和y的值。由于線性插值，這些值中至少有一個是0或1。直覺地說，如果部分地穿過中心單元格要比繞邊界而行花費更少，那麼完全地穿過單元格的開銷最小。是以，如果有任何代價最低的路徑穿過中心網格，它将盡可能大，迫使x = 0或y = 1。如果沒有路徑穿過中心網格，然後y= 0。（詳細證明見論文）
經過論證可知，路徑要麼沿着整個底部邊緣 s 1 s_1 s1(圖5(ii))；或者将移動距離x底部邊緣，然後直接沿着直線路徑s2(圖5(3))；或将s到sy點直線路徑右側邊緣(圖5 (iv))。哪一條路代價低，取決于c，b的相對大小，路徑 s 1 s_1 s1和 s 2 s_2 s2代價差異： f = g ( s 1 ) − g ( s 2 ) f = g(s_1)−g(s_2) f=g(s1)−g(s2)。具體來說，如果f < 0，那麼從s到 s 1 s_1 s1的最優路徑為 m i n ( c , b ) + g ( s 1 ) min(c, b)+g(s_1) min(c,b)+g(s1)(圖5(ii))。如果f = b，那麼使用底部邊緣一部分路徑的代價（圖5(iii)）将等于不使用底部邊緣路徑的成本（圖5(iv)）。可以解出後者的路徑 y ⃗ \vec{y} y
(等于 1 − x ⃗ 1-\vec{x} 1−x
前路徑),最小成本路徑如下。首先，令k = f = b，s的路徑代價為（圖8示例說明）
圖8
上式對y求導，令為0，可得取最小值時的y值：
無論使用底邊還是右邊，最終得到的計算和路徑代價都是相同的。是以重要的是哪條邊代價更低。如果f < b，然後使用上面的右邊緣，并如上計算路徑的代價(當k = f)；如果b < f，使用底部邊緣，并用k = b和 y ⃗ = 1 − x ⃗ \vec{y}=1-\vec{x} y
=1−x
替代到上面的方程。對于任意兩個相鄰的 s a s_a sa和 s b s_b sb，計算s最小的路徑代價算法如圖9所示。
圖9
3.Filed D*算法僞代碼
一旦對圖中給定的節點進行了基于插值的路徑代價計算，然後就可以将其插入許多路徑規劃算法中以生成平滑路徑。圖10給出了Field D_star公式，這是一種包含這些插值路徑代價的增量重新規劃算法。這個版本的Field D_star是基于D_star Lite算法的(不同之處用紅色高亮顯示)。與最初基于圖形的D* Lite版本不同，{20 - 22}行将Field D_star裁剪為網格。同時，由于路徑相交的不僅僅是節點， h ( s s t a r t , s ) h(s_{start}, s) h(sstart,s)必須足夠小，當添加到從s發出的任何邊的代價時，它仍然不大于從 s s t a r t s_{start} sstart到s的最小代價路徑。
圖10
一旦計算出從初始狀态到目标的路徑的代價，就可以從初始位置開始擴充路徑，并疊代計算要移動到下一個位置的單元邊界點。由于插值技術，計算網格單元内任意一點的路徑代價是可能的，而不僅僅是角落，這對于提取路徑和在執行不完美的情況下傳回軌道(通常對于真正的機器人是如此)都是有用的。Field D*應用的兩個例子如下圖11：
圖11
4.算法優化
為了減少計算量，通過在算法中降低被彈出狀态的相鄰點資訊更新(第{09 - 14}行)時需要的計算量，隻考慮那些被彈出狀态的新值實際影響的狀态以及這些狀态是如何受到影響的。解決方案是引入狀态函數bptr(s)、cknbr(s, s’)和ccknbr(s, s’)，和對算法做出兩處修改。最終的算法為：
圖12
5.算法總結
Filed D_star算法是基于D_star Lite算法進行改進的，與D_star Lite算法最大的不同在于路徑搜尋的過程中，搜尋方向不再局限于隻能為π/4的倍數，而可以是任意的方向，可以生成更為平滑的路徑，同時盡可能是最優路徑，而不是次優路徑。但是，該算法同樣存在一定的問題：由于 s y s_y sy的路徑代價不一定符合 g ( s 1 ) g(s_1) g(s1)與 g ( s 2 ) g(s_2) g(s2)的線性組合，線性估計的誤差随路徑點代價的更新累積，導緻插值點位置估計發生偏差，使其前後路徑連線不一定共線。
參考文獻
[1]馬麗莎, 周文晖, 龔小謹, et al. 基于運動限制的泛化Field D~路徑規劃[J]. 浙江大學學報(工學版), 2012, 46(08): 1546-1552.
[2]Ferguson D, Stentz A. The Field D Algorithm for Improved Path Planning and Replanning in Uniform and Non-Uniform Cost Environments[C], 2005.
搜尋算法其他文章
Dstar Lite路徑規劃算法
終身規劃Astar算法（LPA*）：Lifelong Planning A*
D*路徑搜尋算法原了解析及Python實作