趙其定，汪夏榮

（1.江西經(jīng)濟(jì)管理干部學(xué)院飛行技術(shù)學(xué)院，江西 南昌 330088；2.深圳市大疆創(chuàng)新科技有限公司，上海 200120）

引言

無人駕駛飛行器（Unmanned Aerial Vehicle,UAV），也稱為空中無人機(jī)，已經(jīng)出現(xiàn)在許多需要在特定環(huán)境中執(zhí)行一些環(huán)境復(fù)雜重復(fù)性的任務(wù)中。在這些應(yīng)用中需要自主操作，這增加了無人機(jī)導(dǎo)航到任務(wù)區(qū)域的更多要求。導(dǎo)航方法一般可分為全局路徑規(guī)劃、局部路徑規(guī)劃和混合路徑規(guī)劃[1]。

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

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

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

1 問題描述

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

其中d(t)是到最近障礙物的距離，||·||是R3 中向量的標(biāo)準(zhǔn)的歐幾里得范數(shù)。

考慮適用于不同類型的無人機(jī)通用3D 非完整運(yùn)動(dòng)學(xué)模型，該模型的描述如公式（2）-（4）：

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

由于只考慮向前運(yùn)動(dòng)，不允許V(t)為負(fù)值，對(duì)于恒速應(yīng)用，V(t)保持恒定在某個(gè)值V＞0。做出以下假設(shè)。

假設(shè)1:無人機(jī)通過傳感器感知部分環(huán)境，可以確定到最近障礙物的距離，作為感知系統(tǒng)的一部分?？紤]抽象傳感模型，其中可以檢測(cè)到距無人機(jī)dsensing＞0距離內(nèi)的障礙物，使用邊界幾何圖來表示物體的傳感部分。

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

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

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

2 混合導(dǎo)航策略

自主避障導(dǎo)航方法采用的是模塊化結(jié)構(gòu)，從軟件的角度來看，整個(gè)系統(tǒng)結(jié)構(gòu)包括感知、導(dǎo)航和控制子系統(tǒng)。感知子系統(tǒng)負(fù)責(zé)處理機(jī)載傳感器測(cè)量，提供有關(guān)環(huán)境信息?？梢陨森h(huán)境的地圖信息，并根據(jù)反應(yīng)控制方法提供最近障礙物的距離和方向。

高級(jí)導(dǎo)航子系統(tǒng)的設(shè)計(jì)是這項(xiàng)工作的主要貢獻(xiàn)，所提出的設(shè)計(jì)結(jié)合了幾個(gè)主要組件，即全局路徑規(guī)劃、運(yùn)動(dòng)監(jiān)控器、路徑跟蹤控制、反應(yīng)控制和切換機(jī)制。顯示這些組件的整體混合導(dǎo)航策略的架構(gòu)見圖1。

圖1 避障導(dǎo)航策略架構(gòu)

全局路徑規(guī)劃組件根據(jù)當(dāng)前可用的全局地圖生成可行且安全的幾何路徑。每當(dāng)分配新的目標(biāo)位置或發(fā)現(xiàn)環(huán)境的新部分時(shí)檢測(cè)到誘捕情況（即無人機(jī)卡?。r(shí)，運(yùn)動(dòng)監(jiān)控器就會(huì)被觸發(fā)生成新的路徑。在未知和動(dòng)態(tài)的環(huán)境中，無人機(jī)可能會(huì)因?yàn)闄z測(cè)到新的障礙物而發(fā)現(xiàn)規(guī)劃的全局路徑不安全。為了快速響應(yīng)，反應(yīng)控制組件用于將命令速度直接耦合到當(dāng)前傳感器觀測(cè)值來生成圍繞未知/動(dòng)態(tài)障礙物的局部安全運(yùn)動(dòng)，與重新規(guī)劃新路徑相比，反應(yīng)控制對(duì)障礙物的快速反應(yīng)是由于其計(jì)算成本低，然后使用一種機(jī)制根據(jù)傳感器測(cè)量在路徑跟蹤和反應(yīng)式導(dǎo)航之間切換。

這些組件的詳細(xì)描述在以下小節(jié)中給出，任務(wù)計(jì)算機(jī)通常連接到所有電機(jī)的飛行控制單元，然后使用低級(jí)控制子系統(tǒng)生成執(zhí)行器/電機(jī)命令，以執(zhí)行高級(jí)導(dǎo)航子系統(tǒng)生成的高級(jí)快速命令。

2.1 全局路徑規(guī)劃

全局路徑規(guī)劃算法需要環(huán)境的地圖來確定，規(guī)劃路徑的最優(yōu)性和整體算法的復(fù)雜性可能是決定不同應(yīng)用的關(guān)鍵因素。每個(gè)規(guī)劃器在特定條件下使用，基于優(yōu)化算法來生成最佳的路徑，同時(shí)在需要重新規(guī)劃時(shí)實(shí)施計(jì)算效率高的算法來修改初始路徑[7]。

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

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

RRT-C 遵循同樣的思路，通過維護(hù)兩棵樹，來自初始配置和目標(biāo)配置。在每次迭代中，規(guī)劃器嘗試擴(kuò)展其中一棵樹，然后嘗試找到與另一棵樹中一個(gè)頂點(diǎn)的無沖突連接。與標(biāo)準(zhǔn)RRT 算法相比，這為復(fù)雜環(huán)境中解決方案提供快速收斂。

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

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

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

2.2 追蹤路徑跟隨控制

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

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

2.3 反應(yīng)式控制方法

在未知?jiǎng)討B(tài)環(huán)境中導(dǎo)航需要更多的安全措施，如果障礙物是動(dòng)態(tài)的，全局規(guī)劃器的規(guī)劃路徑可能會(huì)變得不安全。反應(yīng)控制法用于通過在檢測(cè)到的障礙物周圍導(dǎo)航來對(duì)檢測(cè)到的障礙物產(chǎn)生類似反射的反應(yīng)，直到可以安全地遵循先前計(jì)劃的路徑。

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

考慮恒定的前進(jìn)速度，d0＞dsafe＞0 是所需 距離，Γ=±1 確定回避機(jī)動(dòng)方向，sgn(α)為符號(hào)函數(shù)。此外χ（β）是一個(gè)飽和函數(shù)，公式(12)定義為：

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

2.4 切換規(guī)則

切換機(jī)制對(duì)于混合避障導(dǎo)航非常重要，負(fù)責(zé)遵循計(jì)劃的路徑是安全的還是被動(dòng)地避開新的障礙物。一般使用兩種導(dǎo)航模式，并采用切換機(jī)制在兩種模式之間切換。這兩種模式是路徑跟隨模式M1 和避障/反應(yīng)模式M2。

假設(shè)無人機(jī)最初以模式M1 啟動(dòng)，考慮以下切換規(guī)則：

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

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

2.5 運(yùn)動(dòng)管理

每當(dāng)無人機(jī)新發(fā)現(xiàn)的環(huán)境而陷入局部最小值時(shí)，運(yùn)動(dòng)監(jiān)控器負(fù)責(zé)檢測(cè)陷阱情況。當(dāng)在類似迷宮的環(huán)境、凹形障礙物中導(dǎo)航時(shí)，解決此問題的方法是向全局路徑規(guī)劃器發(fā)出重新規(guī)劃命令，以根據(jù)有關(guān)環(huán)境的更新信息獲取新路徑[6]。

3 仿真結(jié)果與討論

混合導(dǎo)航方法使用兩種不同情況的模擬進(jìn)行測(cè)試，即靜態(tài)環(huán)境和動(dòng)態(tài)環(huán)境。假設(shè)有關(guān)這些環(huán)境的一些特征是已知的，以顯示全局規(guī)劃的作用。RRT-C 算法是基于三維路徑規(guī)劃實(shí)現(xiàn)的，用于規(guī)劃全局路徑。

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

3.1 未知的靜態(tài)障礙

在第一個(gè)模擬案例中，考慮無人機(jī)執(zhí)行一些重復(fù)性任務(wù)，關(guān)于環(huán)境的特點(diǎn)是已知的，如墻壁位置。無人機(jī)不知道的靜態(tài)障礙物可以在不同時(shí)間添加到環(huán)境中。這種場(chǎng)景的一個(gè)真實(shí)示例是在建筑物內(nèi)操作時(shí)，建筑布局是提前知道的。

圖2 顯示了該仿真案例所考慮的環(huán)境，無人機(jī)可用的初始地圖僅包括一堵墻，這堵墻有兩個(gè)方形缺口。有9 個(gè)未知圓柱形障礙物。在實(shí)踐中，通?？梢酝ㄟ^無人機(jī)的感知系統(tǒng)估計(jì)邊界形狀以確定障礙物。

圖2 混合導(dǎo)航策略規(guī)劃的路徑和執(zhí)行的運(yùn)動(dòng)（靜態(tài)環(huán)境）

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

圖3 無人機(jī)到障礙物的距離

圖4 無人機(jī)運(yùn)動(dòng)中的線速度

3.2 未知的動(dòng)態(tài)障礙

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

圖5 初始規(guī)劃路徑和執(zhí)行運(yùn)動(dòng)（動(dòng)態(tài)環(huán)境）

圖6 初始規(guī)劃路徑和執(zhí)行運(yùn)動(dòng)（動(dòng)態(tài)環(huán)境）

圖7 無人機(jī)到障礙物的距離

圖8 無人機(jī)運(yùn)動(dòng)過程中的線速度

4 結(jié)論

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