Path Finding with Numeric Field Method ~ Pandia's Blog

Motion Planner路徑規劃器 (Qt PC/Android)

Onject/目的

Motion Planner is an important part of robotic. Its goal is finding a path for a robot at a start position to destination in an unknown planar space with static obstacles.

Motion Planner是機器人學裡面常見的路徑規劃器，在陌生且佈滿靜態障礙物的平面空間中，如何讓機器人規劃出到達目的地的路徑。

Path Finding/路徑搜尋流程

Robots and obstacles are composed with polygons in a planar space. We gave each of them a configuration (x, y, θ), so that each polygon defined with local coordinate can be translate and rotate in the world coordinate. Thus, a path is defined as a serial of configurations.

在平面空間中的機器人與障礙物皆為多邊形所構成，每個機器人與障礙物皆為多邊形，依照各自的原點座標繪製，並給予一組由(x, y,θ)構成的組態(Configuration)，根據此組態對應至世界座標，如圖一所示。而路徑則是一連串組態形成的序列。

Fig.1 Object is rotated and translated from local to world coordinate

圖一、物體經過平移旋轉至世界座標示意圖

Besides, we gave each robot several control points as reference points of the heuristic functions used in motion planning.

Once the positions of robots and obstacles are defined, we discretize obstacles and build a Potential Field for each control point of robot. Suppose the destination of a robot is zero, the value of potential field is the distance to destination. In fig.2, the left-most one is a planar map where the purple polygons are obstacles, yellow one is start position of robot while red one is the destination. We can see the middle and right-most part of fig. 2, these are potential field generated with robot’s two control points. The brighter color means lower value, while the darker color means higher value, and the yellow parts are obstacles.

此外每個機器人還會給予若干個控制點，作為路徑規畫時取得效益函數(heuristic function)的基準點。

機器人和障礙物的位置決定後，我們會將障礙物離散化並對每個機器人的控制點建立一個位能場(Potential Field)。建立位能場的方式為以機器人在終點時，控制點對應的位置為0開始向周圍遞增，如圖二最左所示，紫色代表障礙物，黃色代表機器人的起始位置，紅色代表機器人的目的位置。而圖二中間和右邊兩張圖則是以機器人的兩個控制點繪製出的位能場，顏色越亮表示位能場數值越低(右下角)，而越暗表示數值越高，黃色則代表障礙物。

Fig.2 Potential Field

圖二、位能場示意圖

Path finding is pretty similar to A* algorithm. We start from the start point’s configuration and expand it with six directions +/-(x, y,θ). Each configurations will record its parent. And we will filter out those configuration led to collision. We then compute the score of each new configuration with this formula: Sigma( Wn * PFn( CPn ) ) , CPn indicates the n-th control point’s position, PFn( CPn ) is the potential value of CPn , Wn is weight.

We expand the configuration with the lowest score in each iteration until all possible configuration are expanded or the score of a configuration is zero, which means we found a path. The result is shown in Fig. 3.

搜尋路徑方式是一種類似A*演算法的過程。從起點的組態開始，往三個維度上的六個方向展開+/- (x, y,θ)，展開得到的這六個新的組態，每個組態都會記錄展開他的組態為何。過濾掉會造成碰撞的組態後，我們會將機器人帶入這些組態，並計算在這個組態下得到的分數。分數計算方式如此公式所示：Sigma( Wn * PFn( CPn ) ) ， CPn為第 n 個控制點的位置， PFn( CPn )為控制點在 CPn 的位能值，Wn 為權重。也就是將每個控制點所在的位置，取得對應位能場的值並加權相加後得到的分數，並將其記錄下來。演算法每次會展開分數最低的組態，直到所有的組態都搜尋完(找不到路徑)或者是該組態的分數為0為止，如此只要逆推回去即可得到路徑。結果如圖三所示：

Fig.3 Found a path

圖三、搜尋出路徑示意

Computing collision is the most time consuming part of the algorithm. To optimize the algorithm, we can rotate the robot among each possible angle and compute the minkowski sum with each obstacles. That is, we precompute all configurations may cause collision. Then we can consider the robot as a point. During the expanding configuration part of the algorithm, we can simply look up the precompute table and do no more collision detection. Fig.4 is an example of minkowski sum with different angle of robot. Yellow indicates obstacles, red indicates collision, and black part is collision free.

在前述例子中組態展開時的碰撞偵測是最費時的地方，為了加速演算法我們可以將機器人旋轉各種角度後對每個障礙物和計算其minkowski sum，預先劃出可能碰撞的範圍，那麼我們找尋路徑時就可以將機器人假想成是一個點，組態展開時僅需查表，而不需再做碰撞偵測了。圖四是不同角度所畫出的minkowski sum的示意，紅色表示產生碰撞之區域，黑色表示不會產生碰撞的區域，黃色則是障礙物。

Fig.4 、Minkowski Sum with different angle

圖四、不同角度minkowski sum的示意圖

Discussions/討論

A planar map with obstacles may cause lots of local minima, which might affect the iterations time before finding a path. Without minkowski sum, we have to do many times of collision detection, which we need to detect line intersection and plane contains points. It is really time consuming. However, minkowski sum is an algorithm with complexity O(m+n) where m and n are the two polygons’ edge number. Besides, computing minkowski sum has no dependency problem, thus we can use parallel library to compute all the minkowski sum together to reduce the time.

障礙物構成的地圖可能會有許多Local Minima影響搜尋出路徑所需的次數，而不使用minkowski sum時，碰撞偵測必須將機器人與每個障礙物進行線段相交以及面包含點的測試，是最為費時的過程。然而minkowski sum是一個複雜度為O(m+n)的演算法，其中m與n分別代表要計算的兩個多邊形的邊數。而此方法可以前處理先計算好可能會產生碰撞的組態，此外計算的過程也無先後順序的問題，因此可以透過平行處理的方式更為加速此演算法。

In an intel i7-2600k CPU, using tradition collision detection take two times longer than parallel compute minkowski sum. Fig.5 is a motion planner written in C++ and Qt. The search space is sized of 128x128x36 configurations. It costs about 7.2 seconds to find a path in this map with large amount of local minima.

在intel的i7-2600k處理器上，使用傳統碰撞偵測和平行處理方式建立minkowski sum，後者所需的時間僅為前者的一半，甚至更少。而機器人路徑規劃的時間越短則多出來的時間更有能力去做其他幾何運算。圖五是以C++搭配Qt實作的路徑規劃器，在搜尋空間為128x128x36的組態下，測試這張Local Minima極多的圖，也僅需不到7.2秒即可完成路徑規劃。