Field Ｄ*路徑規劃算法

緊接着上一篇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_2$走折線段。根據圖中的計算可知，走$e_1$，$e_2$的路線約多走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$在邊界的值,那麼僅僅可以通過最小化$c(s, s_b)+ g(s_b)$計算節點s的最優值，$c(s, s_b)$通過s和sb之間的距離乘以到達s所在單元的代價。但是，這樣的點有無數個，因此不可能爲每個點計算$g(s_b)$。

圖4

但是，用線性插值法對每個邊界點提供g(sb)的近似是可能的。首先將圖4中的節點視爲連續代價的樣本點，由節點s開始的一條優化路徑必須通過連接兩個連續相鄰的邊緣,例如$\vec{s_1s_2}$。因此，設置通過這些邊的路徑的最小代價爲s的路徑代價，而這些邊被認爲是一次一條。爲了通過$\vec{s_1s_2}$計算節點s的路徑，需要使用節點$s_1$和$s_2$的路徑代價以及圖5（a）中所示中間網格的轉移代價c和底部網格的轉移代價b。

圖5

假設位於$s_1$和$s_2$之間邊界上的任意點sy的路徑代價是$g(s_1)$和$g(s_2)$的線性組合（如圖6）:

圖6

其中，其中y是$s_1$到$s_y$的距離(假設單位單元格)。這個假設並不完美，$s_y$的路徑代價可能不是$g(s_1)$和$g(s_2)$的線性組合，甚至也不是這些路徑代價的函數。然而，這種線性近似在實際應用中效果良好，可以構造節點s路徑代價的封閉形式解。根據這個近似，s在$s_1$、$s_2$、網格代價c和b下的路徑代價可以計算爲

圖7

這個公式可以通過上圖7中的（a）圖理解。公式中的第1項爲s→$s_x$的代價，第2項爲$s_x$→$s_y$的代價，第3、4項爲y處的代價。其中，$x\in[0, 1]$，$y\in[0, 1]$。當x=y=1時，路徑爲底部的路徑，但是代價卻是中間網格的代價。

令$(\vec{x},\vec{y})$是一對解決上述最小化x和y的值。由於線性插值，這些值中至少有一個是0或1。直觀地說，如果部分地穿過中心單元格要比繞邊界而行花費更少，那麼完全地穿過單元格的開銷最小。因此，如果有任何代價最低的路徑穿過中心網格，它將儘可能大，迫使x = 0或y = 1。如果沒有路徑穿過中心網格，然後y= 0。（詳細證明見論文）

經過論證可知，路徑要麼沿着整個底部邊緣$s_1$(圖5(ii))；或者將移動距離x底部邊緣，然後直接沿着直線路徑s2(圖5(3))；或將s到sy點直線路徑右側邊緣(圖5 (iv))。哪一條路代價低，取決於c，b的相對大小，路徑$s_1$和$s_2$代價差異：$f = g(s_1)−g(s_2)$。具體來說，如果f < 0，那麼從s到$s_1$的最優路徑爲$min(c, b)+g(s_1)$(圖5(ii))。如果f = b，那麼使用底部邊緣一部分路徑的代價（圖5(iii)）將等於不使用底部邊緣路徑的成本（圖5(iv)）。可以解出後者的路徑$\vec{y}$(等於$1-\vec{x}$前路徑),最小成本路徑如下。首先，令k = f = b，s的路徑代價爲（圖8示例說明）

圖8

上式對y求導，令爲0，可得取最小值時的y值：

無論使用底邊還是右邊，最終得到的計算和路徑代價都是相同的。所以重要的是哪條邊代價更低。如果f < b，然後使用上面的右邊緣，並如上計算路徑的代價(當k = f)；如果b < f，使用底部邊緣，並用k = b和$\vec{y}=1-\vec{x}$替代到上面的方程。對於任意兩個相鄰的$s_a$和$s_b$，計算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_{start}, s)$必須足夠小，當添加到從s發出的任何邊的代價時，它仍然不大於從$s_{start}$到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$的路徑代價不一定符合$g(s_1)$與$g(s_2)$的線性組合，線性估計的誤差隨路徑點代價的更新累積，導致插值點位置估計發生偏差，使其前後路徑連線不一定共線。

參考文獻

[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實現