在三維未知動態環境中無人機避障導航混合方法的研究

趙其定，汪夏榮

（1.江西經濟管理干部學院飛行技術學院，江西 南昌 330088；2.深圳市大疆創新科技有限公司，上海 200120）

引言

無人駕駛飛行器（Unmanned Aerial Vehicle,UAV），也稱為空中無人機，已經出現在許多需要在特定環境中執行一些環境復雜重復性的任務中。在這些應用中需要自主操作，這增加了無人機導航到任務區域的更多要求。導航方法一般可分為全局路徑規劃、局部路徑規劃和混合路徑規劃[1]。

全局路徑規劃需要對環境有全面的了解，以生成由無人機控制系統跟蹤的最佳有效路徑。有許多不同的技術來解決全局規劃問題，包括路線圖方法、單元分解、勢場、概率路線圖、快速搜索隨機樹和基于優化的技術[2-3]。在未知環境中使用的經典反應方法是動態窗口和曲率速度[4]。一類反應式方法采用邊界跟隨模式來規避障礙物[5]，這種方法的低計算成本是以容易陷入陷阱為代價，將隨機行為與邊界跟隨方法相結合[6]，以規避這種情況。

混合策略傾向于通過結合判斷和反應方法來解決上述缺點，從而在未知和動態環境中實現更有效的導航。本文提出了一種混合3D 導航策略，以允許在部分未知/動態環境中進行有效導航。建議的策略將全局路徑規劃層與基于通3D 運動學模型開發的反應性避障控制方法相結合，基于快速搜索隨機樹混合算法(Rapidly Exploring Random Tree Connecting，RRT-C)的全局路徑規劃層可以根據環境狀況生成路徑。采用滑模技術在反應層中實現邊界跟隨，與基于搜索和基于優化的本地規劃器相比，這種以低廉的計算成本提供了對障礙物的快速反應[5]。為了適應混合導航策略，提出了一種切換機制來實現兩個控制方法之間的轉換。總體而言，所提出的方法可以克服純粹依靠判斷或被動方法的缺點。

本文組織如下，第2 節提供了解決避障導航問題的公式，然后在第3 節中介紹了混合策略，第4 節運用混合策略這種方法的性能通過不同的模擬場景得到證實，最后在第5 節中作出結論。

1 問題描述

這里考慮了一個3D 導航問題，其中需要無人機在已知的環境ε?R3 中安全導航。環境ε 包含n 個障礙物O={O1，O2，…，On}?ε。這些障礙物可以是已知靜態Ok或未知動態Ou使得O=Ok∪Ou。主要目標是引導無人機安全地到達，坐標系W 中表示的某個目標位置pgoal∈ε{O}。UAV 從環境的初始位置開始，該地圖僅包含基于先前關于Ok的信息。也可以考慮未知環境，無人機在穿越環境時可以開始構建地圖。在這種情況下，最初Ok=?。另外設p(t)=[x(t), y(t), z(t)]T為以W 表示無人機的笛卡爾坐標。注意將向量表示為列向量，寫成行向量[·]T 的轉置以簡化書寫。無人機的安全要求定義為與所有障礙物保持安全距離dsafeε＞0，具體如公式（1）所示：

其中d(t)是到最近障礙物的距離，||·||是R3 中向量的標準的歐幾里得范數。

考慮適用于不同類型的無人機通用3D 非完整運動學模型，該模型的描述如公式（2）-（4）：

其中V(t)∈R+是線速度，u(t)∈R3是與角速度相關的二自由度控制輸入, 任何給定時刻t 的運動方向由單位矢量r(t)表征。公式(4)表明輸入u(t)始終垂直于r(t)，這會在3D 中產生類似轉向的行為。由于物理限制，無人機速度V(t)和u(t)被視為具有一些上限的控制輸入, 由Vmax，umax＞0 表示，這些約束可以表示如公式(5)：

由于只考慮向前運動，不允許V(t)為負值，對于恒速應用，V(t)保持恒定在某個值V＞0。做出以下假設。

假設1:無人機通過傳感器感知部分環境，可以確定到最近障礙物的距離，作為感知系統的一部分。考慮抽象傳感模型，其中可以檢測到距無人機dsensing＞0距離內的障礙物，使用邊界幾何圖來表示物體的傳感部分。

假設2:估計無人機的位置p(t)和方向矢量r(t)，以下陳述總結了這項工作中考慮的問題。

問題1:考慮無人機其運動可以用公式(2)-(4)來描述。在假設1 和2 下，設計V(t)和u(t)的控制方法，確保通過未知部分或已知環境ε 無碰撞導航，通過滿足公式(1)中的安全要求到達目標位置pgoal和公式(5)中的約束。

備注1:公式(2)-(4)可適用于固定翼無人機、多旋翼無人機。

2 混合導航策略

自主避障導航方法采用的是模塊化結構，從軟件的角度來看，整個系統結構包括感知、導航和控制子系統。感知子系統負責處理機載傳感器測量，提供有關環境信息。可以生成環境的地圖信息，并根據反應控制方法提供最近障礙物的距離和方向。

高級導航子系統的設計是這項工作的主要貢獻，所提出的設計結合了幾個主要組件，即全局路徑規劃、運動監控器、路徑跟蹤控制、反應控制和切換機制。顯示這些組件的整體混合導航策略的架構見圖1。

圖1 避障導航策略架構

全局路徑規劃組件根據當前可用的全局地圖生成可行且安全的幾何路徑。每當分配新的目標位置或發現環境的新部分時檢測到誘捕情況（即無人機卡住）時，運動監控器就會被觸發生成新的路徑。在未知和動態的環境中，無人機可能會因為檢測到新的障礙物而發現規劃的全局路徑不安全。為了快速響應，反應控制組件用于將命令速度直接耦合到當前傳感器觀測值來生成圍繞未知/動態障礙物的局部安全運動，與重新規劃新路徑相比，反應控制對障礙物的快速反應是由于其計算成本低，然后使用一種機制根據傳感器測量在路徑跟蹤和反應式導航之間切換。

這些組件的詳細描述在以下小節中給出，任務計算機通常連接到所有電機的飛行控制單元，然后使用低級控制子系統生成執行器/電機命令，以執行高級導航子系統生成的高級快速命令。

2.1 全局路徑規劃

全局路徑規劃算法需要環境的地圖來確定，規劃路徑的最優性和整體算法的復雜性可能是決定不同應用的關鍵因素。每個規劃器在特定條件下使用，基于優化算法來生成最佳的路徑，同時在需要重新規劃時實施計算效率高的算法來修改初始路徑[7]。

混合避障導航策略不限于特定的路徑規劃算法，將基于抽樣的方法視為實現全局規劃器的關鍵步驟，規劃器在三維空間中進行規劃時可以很好地擴展。使用了快速搜索隨機樹（Rapidly Exploring Random Tree，RRT）算法的一種變體[8]，稱為快速搜索隨機樹混合算法(RRT-C)。由于算法的流行和低計算成本，每當檢測到陷阱時，需要在運動期間進行在線規劃。

RRT 算法是基于隨機抽樣的路徑規劃方法，其基本概念總結如下，G 是用初始配置初始化的搜索樹，使用迭代方法在配置空間中擴展搜索樹，直到找到可行的解決方案。在每次迭代中，隨機或使用一些啟發式對配置進行采樣，這有助于使搜索樹的增長。發現偏向抽樣過程選擇具有一定概率(0＜pbgoal＜1)的目標配置以增強RRT 增長，然后該算法通過將搜索樹G 連接到樹中最近的配置來將其擴展到采樣配置[8]。一種方法是使用直線連接兩個配置，在處理時考慮的歐幾里得空間，然后使用碰撞檢查器根據可用的環境圖檢查每個擴展的可行性，每個可行的擴展通過添加新配置導致搜索樹的增長。抽樣過程不斷重復，直到找到初始配置和目標配置之間的可行路徑或滿足停止標準（例如超過預定義的計劃時間）[9]。

RRT-C 遵循同樣的思路，通過維護兩棵樹，來自初始配置和目標配置。在每次迭代中，規劃器嘗試擴展其中一棵樹，然后嘗試找到與另一棵樹中一個頂點的無沖突連接。與標準RRT 算法相比，這為復雜環境中解決方案提供快速收斂。

由于RRT 算法的采樣性質，生成的路徑在總長度方面不是最優。在擴展搜索樹時考慮直線，這些路徑不滿足非完整約束，因此通過兩個后處理階段（即修剪和平滑）細化獲得路徑[10]。

在剪枝階段，從規劃路徑中去除多余的航路點，以提高整體路徑質量。令W={w1,w2,···,wk}為一組節點，表示RRT-C 生成的路徑，令Wp為經過此階段后得到的剪枝路徑，然后用算法1 刪除冗余航路點。

在修剪后的路徑旁邊應用平滑算法，以確保最終路徑滿足無人機的非完整約束（最小曲率半徑），使用參數貝塞爾曲線生成連續曲率平滑路徑。

2.2 追蹤路徑跟隨控制

為了確保無人機準確跟蹤規劃路徑需要進行路徑跟隨控制設計，采用純追蹤算法（Pure Pursuit Tracking，PP），該算法以穩定簡單[9]。假設幾何路徑τ?R3 可用，PP 算法引導無人機跟隨虛擬目標pv=(xv，yv，zv)∈τ 沿路徑移動。這個目標被選擇在距離最近的路徑點pc∈τ 到無人機當前位置的某個前沿距離L 處。當無人機遠離此處的計劃路徑時，PP 算法提供更高的穩定性，因為無人機有時會在使用反應控制組件避開障礙物時偏離計劃路徑，公式(6)-(7)控制算法用于路徑跟蹤：

其中γ＞0。映射函數F(w1，w2):R3×R3→R3作為一個轉向函數，它產生一個在w2方向上垂直于w1的向量，具體定義見公式(8)：

2.3 反應式控制方法

在未知動態環境中導航需要更多的安全措施，如果障礙物是動態的，全局規劃器的規劃路徑可能會變得不安全。反應控制法用于通過在檢測到的障礙物周圍導航來對檢測到的障礙物產生類似反射的反應，直到可以安全地遵循先前計劃的路徑。

采用基于滑動模式控制技術的反應控制算法[10]，到最近障礙物的距離d (t) 是實現該控制器所需的信息，且可以從無人機傳感器獲得，這種反應控制基于邊界跟隨模式，如公式(9)-(10)所示：

考慮恒定的前進速度，d0＞dsafe＞0 是所需 距離，Γ=±1 確定回避機動方向，sgn(α)為符號函數。此外χ（β）是一個飽和函數，公式(12)定義為：

對于某些設計參數γ，δ＞0。ia(t*)表示與某個障礙物的回避平面法線，這個法線對于每個障礙物可能是不同的，并且可以在檢測到新障礙物的時刻t*確定。反應式導航方法確保無人機保持固定距離d0，同時在某些假設下繞過最近的障礙。

2.4 切換規則

切換機制對于混合避障導航非常重要，負責遵循計劃的路徑是安全的還是被動地避開新的障礙物。一般使用兩種導航模式，并采用切換機制在兩種模式之間切換。這兩種模式是路徑跟隨模式M1 和避障/反應模式M2。

假設無人機最初以模式M1 啟動，考慮以下切換規則：

規則一：當與最近障礙物Oi的距離低于某個閾值距離C 即i.e.,d (t)=C 和d (t)＜0 時，切換到反應模式M2。

規則二：當|d(t)-d0|＜ε0時從M2 切換到路徑跟隨模式M1 對于某個小的值ε0＞0 并且無人機的航向是針對規劃路徑上的虛擬目標pv的，可以根據公式(13)確定：

2.5 運動管理

每當無人機新發現的環境而陷入局部最小值時，運動監控器負責檢測陷阱情況。當在類似迷宮的環境、凹形障礙物中導航時，解決此問題的方法是向全局路徑規劃器發出重新規劃命令，以根據有關環境的更新信息獲取新路徑[6]。

3 仿真結果與討論

混合導航方法使用兩種不同情況的模擬進行測試，即靜態環境和動態環境。假設有關這些環境的一些特征是已知的，以顯示全局規劃的作用。RRT-C 算法是基于三維路徑規劃實現的，用于規劃全局路徑。

在仿真中對于第一種和第二種情況，分別考慮了V=0.75 m/s 的線速度和umax=1.75 rad/s 和2.5 rad/s 的角速度。設計參數也選擇如下：L=0.5,δ=0.5,γ=1,d0=1和C=2。對公式(11)中使用的ia(t*) 進行了任意選擇，每個選項都不同障礙。此外控制的更新速率設置為0.01 s，接下來介紹仿真結果。

3.1 未知的靜態障礙

在第一個模擬案例中，考慮無人機執行一些重復性任務，關于環境的特點是已知的，如墻壁位置。無人機不知道的靜態障礙物可以在不同時間添加到環境中。這種場景的一個真實示例是在建筑物內操作時，建筑布局是提前知道的。

圖2 顯示了該仿真案例所考慮的環境，無人機可用的初始地圖僅包括一堵墻，這堵墻有兩個方形缺口。有9 個未知圓柱形障礙物。在實踐中，通常可以通過無人機的感知系統估計邊界形狀以確定障礙物。

圖2 混合導航策略規劃的路徑和執行的運動（靜態環境）

圖2 是在靜態環境中混合導航策略規劃的路徑和執行的運動，可以看出所提出的混合導航策略安全地引導無人機從某個初始位置到達目標位置。該圖顯示了全局規劃器基于已知環境初始生成的路徑以及無人機實際執行的路徑。只要無人機與障礙物有良好的間隙，就可以成功地執行計劃路徑。一旦無人機檢測到障礙物，無人機就會切換到反應模式M2 以繞過障礙物。圖3 給出了運動過程中到最近障礙物的距離，表明無人機可以通過與所有障礙物保持適當的間隙來滿足安全要求。無人機的線速度如圖4 所示，結果證實了所提出的方法可以在未知的靜態障礙物之間安全地引導無人機。

圖3 無人機到障礙物的距離

圖4 無人機運動中的線速度

3.2 未知的動態障礙

初始規劃路徑和執行運動在動態環境中如圖5所示，其中A、B、C、D 為四個動態障礙，紅色虛線為初始路徑。全局規劃器使用RRT-C 找到一條安全路徑，如圖6 中的黑線所示。無人機開始以M1 模式移動以跟蹤規劃（參考）路徑，由于環境的動態性，只要在運動過程中有障礙物接近無人機，這條路徑就會變得不安全。每次檢測到有威脅時，無人機根據切換規則切換到規則二的導航。可以在圖6 中看到，這表明無人機的實際執行路徑在某些位置偏離了規劃路徑，以避開動態障礙物。這在圖7 中得到驗證，與最近障礙物的距離保持在安全閾值之上。無人機的線速度如圖8所示。從結果可以看出，所提出的策略在動態環境中也能很好地工作，需要說明的是，無人機的最大速度必須大于障礙物的速度才能保證安全。

圖5 初始規劃路徑和執行運動（動態環境）

圖6 初始規劃路徑和執行運動（動態環境）

圖7 無人機到障礙物的距離

圖8 無人機運動過程中的線速度

4 結論

本文提出了一種用于無人機的混合三維導航策略。問題制定考慮了適用于不同無人機類型和自主水下航行器的通用3D 非完整運動學模型。使用了基于RRT-C 算法的全局規劃器，該算法在三維空間中運行良好，采用基于滑模控制的反應控制方法來避開未知和動態障礙物。仿真結果證實，所提出的混合導航方法在未知靜態和動態障礙物的3D 環境中運行良好。