Timed-Elastic-Band局部路徑規劃算法

早前做工程時嘗試了teb局部規劃算法，覺得效果非常好。由於時間關係，並未深入瞭解，僅依靠做對比實驗調節作者給出的接口參數滿足工程需求。抽空看了一下作者關於teb算法的論文，記錄一些筆記。

teb局部路徑規劃算法github地址：https://github.com/rst-tu-dortmund/teb_local_planner。

作者列出的幾篇文章均推薦閱讀了解。本文基於《Trajectory modification considering dynamic constraints of autonomous robots》一文。

關於time eletic band這個概念，在這推薦看一下qqfly的文章，非常生動形象：link

qqfly文章中關於eletic band（橡皮筋）的定義：連接起始、目標點，並讓這個路徑可以變形，變形的條件就是將所有約束當做橡皮筋的外力。關於time eletic band的簡述：起始點、目標點狀態由用戶/全局規劃器指定，中間插入N個控制橡皮筋形狀的控制點（機器人姿態），當然，爲了顯示軌跡的運動學信息，我們在點與點之間定義運動時間Time，即爲Timed-Elastic-Band算法。

以下爲《Trajectory modification considering dynamic constraints of autonomous robots》的閱讀筆記。

默認的環境爲二維差動輪機器人。

一、Timed Elastic Band概述

定義機器人位姿：

$\chi _{a}=(x_{i},y_{i},\beta _{i})^{T}\in \mathbb{R}^{2}\times S^{1}$ ；

其中 $x_{i}$ ， $y_{i}$ 和 $\beta _{i}$ 分別對應機器人在map座標系（或者世界座標系）的位置和姿態，文章中稱其爲configuration。

則空間內configuration序列（如figure 1）記錄如下：

$Q=\left \{ \chi _{i} \right \}_{i=0,1,...,n} n\in \mathbb{N}$ （1）

兩個configuration間的時間間隔定義爲： $\Delta T_{i}$ ，表示機器人由一個configuration運動到另一個configuration所需時間；

記錄時間序列：

$\tau =\left \{ \Delta T_{i} \right \}_{i=0,1,...,n-1}$ （2）

將configuration及時間序列合併：

$B:=\left ( Q,\tau \right )$ （3）

通過加權多目標優化獲取最優的路徑點，即最優的Q：

$f\left ( B \right )= \sum_{a}^{ }\gamma _{k}f_{k}\left ( B \right )$ （4）

$B^{\ast }=\arg\underset{B}{} minf\left ( B \right )$ （5）

其中 $B^{\ast }$ 爲最優結果， $f\left ( B \right )$ 爲考慮各種約束的目標函數， $f_{k}$ 爲個目標函數的權值。

在此應注意，每個目標函數只與elastic band中的某幾個連續狀態有關，而非整條band。故優化爲對稀疏矩陣模型的優化。具體細節請參考《Efficient trajectory optimization using a sparse model》。

作者在撰寫論文時ROS對稀疏矩陣優化問題的支持並未十分完善，故作者採用分段連續，可微分的代價函數計算破壞約束的懲罰值。

$e_{\Gamma }\left ( x,x_{r},\epsilon ,S,n \right )\simeq\left\{\begin{matrix} \left ( \frac{x-\left ( x_{r}-\epsilon \right )}{S} \right )^{n}, if\, \, \, x > x_{r}-\epsilon \\0 ,otherwise \end{matrix}\right.$ （6）

其中 $x_{r}$ 爲限界值， $S,n,\epsilon$ 影響近似的準確度。具體地說，表示縮放，n表示多項式階數， $\epsilon$ 表示限界值附近一個小位移。

以figure 2爲例。限界值 $x_{r}=0.4$ ，即不應超過0.4。途中ideal constraint表現爲x<0.4時，代價值爲0，當x=0.4時懲罰代價值無窮大。Approximation 1的參數爲： $n=2,S=0.1,\epsilon =0.1$ ；Approximation 2的參數爲 $n=2,S=0.05,\epsilon =0.1$ 。兩項均從x=0.3開始代價函數懲罰起作用。理論上Approximation 1在x>0.4時函數仍起作用，但Approximation 2能有效防止x越界。

二、約束目標函數

1、跟隨路徑和避障約束

約束主要有兩個目標：跟隨已知的全局規劃路徑和避障。兩個目標函數均十分相似，跟隨路徑施力將elastic bands拉向全局路徑，而避障約束施力使得elastic bands遠離障礙物。configuration序列與全局路徑點序列或障礙物 $z_{j}$ 的最近距離記爲 $d_{min,j}$ ，如figure 3所示。跟隨路徑目標以configuration距全局路徑的允許最大距離 $r_{p_{max}}$ 作爲約束，避障目標以configuration距障礙物的允許最小距離 $r_{o_{min}}$ 作爲約束。

實際上原文約束爲attainment of the intermediate way points of the original path，是否爲我所理解的跟隨全局規劃路徑還有待商榷。

約束以懲罰函數實現：

$f_{path}=e_{\tau }\left ( d_{min,j},r_{p_{max}},\epsilon ,S,n \right )$ （7）

$f_{ob}=e_{\tau }\left ( -d_{min,j},-r_{o_{min}},\epsilon ,S,n \right )$ （8）

值得注意的是，目標函數的梯度可視爲對elastic band施加的外力。

2、速度和加速度約束

由速度和加速度組成的動力學約束可以用類似1中運動學約束的懲罰函數表示。以figure 3爲例，機器人運動的平均線速度和角速度可以通過相鄰的configuration $\chi _{i},\chi _{i+1}$ 和時間間隔 $\Delta T$ 計算得到。

$v_{i}\simeq \frac{1}{\Delta T}\left \| \bigl(\begin{smallmatrix} x_{i+1}-x_{i}\\ y_{i+1}-y_{i} \end{smallmatrix}\bigr) \right \|$ （9）

$\omega_{i}=\frac{\beta _{i+1}-\beta _{i}}{\Delta T}$ （10）

類似的，線速度約束可以表示爲：

$f_{v_{i}}=e_{\tau }\left ( v_{i},v_{max},\epsilon ,S,n \right )$

機器人的平均線加速度用同樣的近似計算得到：

$a_{i}=\frac{2\left ( v_{i+1}-v_{i} \right ))}{\Delta T_{i}+\Delta T_{i+1}}$ （11）

角加速度同理可計算得到。

以差動機器人爲運動模型，兩輪的轉速可通過以下計算得到：

$v_{\omega _{r},i}=v_{i}+L\omega _{i}$ （12）

$v_{\omega _{l},i}=v_{i}-L\omega _{i}$ （13）

其中L爲兩輪軸距的一半。

對（12）（13）式作關於時間的微分得到兩輪的加速度。

則可結合機器人設計的機械參數得到的輪子速度和加速度的上下界確定約束上下界。

3、Non-holonomic運動學約束

差動機器人在平面運動只有兩個自由度，其只能以朝向的方向直線運動或旋轉。這種運動學約束使得機器人以有若干弧段組成的平滑的軌跡運動。相鄰的兩個configuration應在弧段的兩端，如figure 4。

初始configuration $\chi _{i}$ 與運動方向 $d_{i,i+1}$ 的夾角 $\theta _{i}$ 應與結束configuration對應的夾角 $\theta _{i+1}$ 相等。

若 $\beta _{i}$ 爲機器人在第i段弧段相對於世界座標系的絕對姿態，則有：

$\theta _{i}=\theta _{i+1}$ （14）

$\Leftrightarrow \begin{pmatrix} \cos \beta _{i}\\ \sin \beta _{i} \\ 0 \end{pmatrix}\times d_{i,i+1}=d_{i,i+1}\times \begin{pmatrix} \cos \beta _{i+1}\\ \sin \beta _{i+1} \\ 0 \end{pmatrix}$ （15）

其中，運動方向向量：

$d_{i,i+1}:=\begin{pmatrix} x_{i+1}-x_{i}\\y_{i+1}-y_{i} \\ 0 \end{pmatrix}$ （16）

相應的目標函數：

$f_{k}\left ( x_{i},x_{i+1} \right )=\left \| \left [ \begin{pmatrix} \cos \beta _{i}\\ \sin \beta _{i} \\ 0 \end{pmatrix}+\begin{pmatrix} \cos \beta _{i+1}\\ \sin \beta _{i+1} \\ 0 \end{pmatrix} \right ] \times d_{i,i+1}\right \|^{2}$ （17）

要注意機器人運動在180度附近的越界的情況。

4、最快路徑約束

目標函數即爲最小化時間間隔序列的二次方。

$f_{k}=\left ( \sum \Delta T_{i} \right )^{2}$ （18）

目標函數使得機器人獲得最快路徑，路徑上的各configuration點在時間上均勻分開，而非傳統的空間上求最短路徑。

三、teb實現

teb的控制流程如figure 5。

在initialization階段，通過增加滿足運動學和動力學約束默認的時間信息將an initial path添加到an initial trajectory（不太理解）。

每次迭代過程中，算法動態添加新的configuration，刪除舊的configuration以將關於空間和時間的分辨率調整至餘下的軌跡長度和規劃的範圍內。添加一個hysteresis實現避障。優化問題轉化爲一個hyper-graph問題，通過g2o中關於大規模稀疏矩陣的優化算法解決。

teb優化問題（公式4）可以轉化爲hyper-graph問題。configurations和以及時間間隔作爲nodes，目標函數 $f_{k}$ 以及其他約束函數爲edges，各nodes由edges連接起來構成hyper-graph。該graph中，每一個約束都爲一條edge，並且每條edge允許連接的nodes的數目是不受限制的。

圖6（a）中展示了兩個configurations $\left ( s_{0},s_{1} \right )$ ，一個時間間隔 $\Delta T_{0}$ ，一個障礙點 $o_{1}$ 組成的hyper-graph。速度約束需要計算平均速度，則該約束與 $s_{0},s_{1},\Delta T_{0}$ 相關，該edge將三個nodes連接起來。障礙物約束將障礙物和離障礙物最近的configuration連接起來。注意障礙物點是固定的，算法不應也不能對其位置進行優化。