一種基於Cubic-Bezier的實時避障平滑路徑規劃方法

作者：由絨褲不是秋褲發表于舞蹈時間：2021-12-18

前幾天在做AI飛行物的路徑規劃（自動尋路）功能，對所用的方法進行一下總結。

本文提供一種將由實時生成的導航路徑點組成的折線路徑轉換為G1或C1連續的Cubic-Bezier拼接曲線的方法，並同時消除（大部分）轉換帶來的碰撞風險。

不涉及：

路徑點的生成

初始切線的制定規則

三次曲線的四點控制可以同時精確描述切入和切出時的位置引數，對於應用於遊戲的三維空間實時姿態控制來說，G1或C1連續的三次拼接曲線已經基本可以滿足絕大部分需求。由於實時規劃的侷限性，無法做到在動態避障的同時（採用不迴圈迭代的方法）利用三次貝塞爾曲線達到G2或C2連續。四次曲線是可以做到的，但是不在本文討論範圍。

二維空間的路徑點和由此拼接成的線段路徑和C1平滑的貝塞爾曲線

出於對效能的考量，應用於遊戲的路徑規劃一般都採用確定可行的折現路徑點，然後再對這個折現進行最佳化模擬出近似的平滑路徑效果的方法。

在由折現轉換為三次曲線的過程中，曲線變換所帶來的偏移會帶來新的碰撞風險。使用路徑點的尋路方法中，只有路徑點之間連線成的折線是安全的，所以一切偏離於折現的部分都要重新評估。

稍微鋪墊下貝塞爾相關方程：

$P\left( t \right) = \sum_{n}^{i}{P_{i}B_{i,n}(t)}$

$B_{i,n}(t) = C_{n}^{i}\bullet t^i(1-t)^{n-i}$

$C_{n}^{i}=\frac{n!}{i!\ast(n-i)!}$

t代表從起點到取樣點的弧長除以總弧長，也等於每個相鄰節點組成的線段上的取樣點掃過的距離的比例。所以

$t\in[0,1]$

$B_{i,n}(t)$

是Bernstein基函式，且正好等於順序無關成功率為t的事件進行n次，成功了i次的機率。它在曲線方程中出現看似巧合其實是必然。它是由貝塞爾曲線各線段取樣點的過渡性質決定的。

綠線代表貝塞爾曲線控制點組成的折線

三次拼接貝塞爾曲線的優勢：

可單獨對每一段曲線進行調整而不影響其他曲線段。路徑點確定後可按需生成路徑曲線：只生成當前取樣點所在曲線段即可。這意味著即使遇到每幀更新的極端情況，每幀需要計算和規劃的三次曲線也只有當前這一段而已。

三維空間三次貝塞爾曲線的“正檢視”

三維空間三次貝塞爾曲線的“側檢視”。四個控制點按順序依次為1234。

為了檢測曲線段是否有碰撞，可以採用生成凸包的碰撞檢測法。上圖是完整路徑規劃中分離出的一段三次貝塞爾曲線段，它由四個控制點組成。可以發現上圖這個由四個控制點組成的四面體就是一個凸包，但是它在2和3兩個頂點附近有大量的空間其實並不“包含”曲線。因此可以稱這兩點附近是“稀疏”的。在很多情況下，誤差都對“深度”更加敏感，即“無效的尖角”所到達的最遠距離對誤差的影響要比比在“非稀疏體積”附近的無效體積能造成的誤差更大，即使這個“無效的尖角”佔用了更少的體積。所以這種尖角是必須要被剔除的。

此時可以運用一些數學技巧找出這個四面體削掉兩個角形成的直角稜臺。使用此稜臺進行碰撞檢測，可有效提高碰撞檢測的精度。

稜臺的製作過程：

設當前貝塞爾曲線的首尾引數點的世界空間座標為P0，P1。

由起始引數點指向相鄰引數點的向量為T0，由次末引數點指向末尾引數點的向量為T1。

則四個引數點的空間座標分別為：P0，P0+T0，P1-T1，P1。

帶入貝塞爾曲線函式

$P\left( t \right) = \sum_{n}^{i}{P_{i}B_{i,n}(t)}$

，其中n=3，

$i\in[0,n]$