# Navigation2 介紹

## Navigation2流程
1. 地圖提供:提供預建好的地圖或使用SLAM建圖。
2. 初始定位:根據地圖獲取機器人初始姿態。
3. 全局規劃:根據目標計算全局路徑。
4. 局部規劃:根據全局路徑和障礙計算局部路徑。
5. 控制器:生成速度指令控制機器人運動。
6. 自主避障:感知障礙並局部重新規劃路徑。
7. 姿態估計:結合里程計、IMU等資料進行定位。
8. 適應地圖:使用動態地圖管理環境變化。
9. 行為控制:協調各組件,實現高層任務控制。

## Navigation2 使用算法
1. **尋路演算法**
* A* 演算法:在格點地圖上進行全局路徑規劃,求最小代價路徑。
* Dijkstra 演算法:解決單源最短路徑問題,用於全局路徑規劃。
* DWA演算法:動態窗口方法,進行局部動態規劃,避開障礙物。
2. **滑動窗口估計**：是移動機器人中的一種姿態估計方法,常用於融合機器人的里程計和IMU傳感器資料。滑動窗口估計的特點是可以有效抑制里程計數據積分漂移的誤差,提高姿態估計的準確性,被廣泛應用於衛星定位輔助或純視覺惡劣環境下的移動機器人中。其基本原理為下列
* 建立優化窗口:保存最新的一系列的傳感器數據(里程計、IMU等)。
* 運動模型:定義傳感器數據與機器人位姿之間的運動模型。
* 誤差項:計算傳感器實際觀測與運動模型預測之間的誤差。
* 最小化誤差:通過最優化算法(例如高斯牛頓法),在優化窗口內最小化誤差,估計機器人當前最佳位姿。
* 滑動窗口:移除最老的傳感器資料,添加最新的資料,保持窗口長度固定,進行周期性姿態更新。

3. **控制演算法**
* 線性二次高斯控制:根據速度指令控制機器人運動。
4. **地圖表示**
* 概率佔用網格地圖:表示環境中障礙物分布的概率。

5. **地圖改進演算法**
* 動態地圖:管理地圖中動態障礙物的存在概率。
## Navigation2 Behavior tree 介紹
Navigation2 Behavior tree如下圖：
這棵樹可以分成兩個較小的子樹，我們可以一次只專注於一個子樹。這些較小的子樹是最頂層的子樹RecoveryNode開始。從此時起，NavigateWithReplanning子樹將被稱為Navigation subtree，而子RecoveryFallback樹將被稱為Recovery subtree。這可以用以下方式表示：
Navigation subtree主要涉及實際的導航行為：

* ComputePath ToPose
* FollowPath
* 上述每個主要導航行為的contextual recovery subtree

此Recovery subtree包括系統層級故障的行為或內部不易處理的項目。
整個 BT（希望）將大部分時間花在Navigation subtree上。如果子樹中的兩個主要行為中的任何一個Navigation失敗（路徑計算或路徑追蹤），將嘗試contextual recovery subtree。
如果contextual recovery subtree仍不夠，Navigation子樹將返回FAILURE。系統將移至Recovery Subtree以嘗試清除任何系統級導航故障。
這種情況會一直發生，直到超過RecoveryNode中number_of_retries賦予的值（預設為 6）。
### Navigation subtree
現在我們已經了解了Navigation subtree和Recovery subtree之間的控制流，讓我們專注於Navigation subtree。
此subtree有兩個主要操作ComputePathToPose和FollowPath。如果這兩個操作中的任何一個失敗，它們將嘗試根據contextual recovery subtree清除故障。樹的關鍵可以只用一個父節點和兩個子節點來表示，如下：
父PipelineSequence節點允許ComputePathToPose被勾選，一旦成功，FollowPath就被勾選。當FollowPath子樹被勾選時，ComputePathToPose子樹也會被勾選。這允許在機器人移動時重新計算路徑。

ComputePathToPose和FollowPath遵循相同的一般結構。
* Do the action
* 如果失敗，請嘗試查看是否可以根據contextual recovery subtree恢復
#### ComputePathToPose subtree：
此subtree只要說明父級節點RecoveryNode和contextual recovery subtree之間的流程。contextual recoveries對於ComputePathToPose subtree和FollowPath subtree功能是檢查是否GoalUpdated和Clear Costmap。

如果您的應用程式在進行系統級復原之前可以容忍更多的上下文復原嘗試，請考慮變更number_of_retries父控制節點中的參數。RecoveryNode

**ComputePathToPose subtree中清除的costmap**
* ComputePathToPose subtree清除GlobalCostmap。GlobalCostmap是contextual recovery subtree的相關成本圖
* FollowPath subtree清除LocalCostmap。LocalCostmap是contextual recovery subtree的相關成本圖
### Recovery Subtree
Recovery subtree是Nav2預設navigate_to_pose_w_replanning_and_recovery.xml樹的第二大「一半」。簡而言之，當Navigation subtree返回時觸發該子樹並控制系統層級的recoveries（在Navigation subtree中的contextual recoveries的FAILURE情況下）
最頂層的父節點ReactiveFallback控制系統範圍內其餘recovery之間的流程，並非同步檢查是否已收到新目標。如果在任何時候目標被更新，該子樹將停止所有子樹並返回SUCCESS。這允許對新目標做出快速反應並搶佔當前正在執行的recovery。這對於Navigation子樹的contextual recoveries部分來說應該很熟悉。這是處理“除非發生‘這種情況’，否則執行操作 A”情況的常見 BT 模式。

這些條件節點可能非常強大，並且通常與 配對ReactiveFallback。很容易想像將整個navigate_to_pose_w_replanning_and_recovery樹包裹在一個ReactiveFallback條件中isBatteryLow- 這意味著navigate_to_pose_w_replanning_and_recovery樹將執行，除非電池電量變低（然後整個不同的子樹用於對接充電）。

如果目標從未更新，行為樹將繼續到該RoundRobin節點。BT 中預設的四個系統級恢復是：

* 清除兩個costmap（Global and local）的序列
* Spin action
* Wait action
* BackUp action

對於SUCCESS父級的四個子級中的任何一個RoundRobin，機器人將嘗試在Navigation子樹中重新導航。如果這次重命名不成功，RoundRobin則會勾選 的下一個子項。

例如，假設機器人被卡住並且Navigation子樹返回FAILURE: （為了這個範例，我們假設目標從未更新）。

1. Recovery嘗試子樹中的 Costmap 清除序列，並傳回SUCCESS。機器人現在再次移動到Navigation subtree

2. 假設清除兩個成本圖還不夠，Navigation subtree再次回傳。FAILURE機器人現在勾選Recovery subtree

3. 在Recovery子樹中，該Spin操作將被勾選。如果返回SUCCESS，則機器人將返回the main Navigation subtree，但我們假設該Spin操作返回FAILURE。在這種情況下，樹將保留在Recovery subtree中

4. 假設下一個操作Wait返回SUCCESS。然後機器人將移動到Navigation subtree

5. 假設Navigation subtree返回FAILURE（清除成本圖、嘗試旋轉和等待仍然不足以恢復系統。機器人將移動到Recovery subtree上並嘗試執行該BackUp操作。假設機器人嘗試了該BackUp操作並能夠成功完成動作BackUp節點返回SUCCESS，所以現在我們再次進入Navigation subtree。

7. 在這個假設場景中，我們假設該BackUp動作允許機器人在Navigation subtree中成功導航，並且機器人到達目標。這樣的話，整體BT還是會回歸的SUCCESS。

如果BackUp action不足以讓機器人擺脫卡住，則Navigation subtree和Recovery subtree邏輯將無限期地繼續下去，直到超過number_of_retries的設定次數，或者如果Recovery subtree中的所有系統範圍恢復都返回（這不太可能，並且可能表示存在其他一些系統故障）。

## 自訂Behavior Tree控制AMR移動
{%youtube a0ve2CH245Y %}