改進RRT 算法的四旋翼無人機路徑規劃方法

馮迎賓，趙子君，晏佳華

(沈陽理工大學自動化與電氣工程學院，沈陽 110159)

由于四旋翼無人機具有靈活輕便、機動性好、動力強等特點，被廣泛應用在多個領域。 當無人機在執行災害救援、軍事偵察等實際應用任務時，飛行路徑上可能存在多種障礙物，為保證無人機安全，四旋翼無人機應具備自主避障能力。 實時的路徑規劃是四旋翼無人機實現自主避障的關鍵技術，其目的是在一定范圍內規劃出一條從起點到終點避開障礙物并適合無人機飛行的路徑。

目前，學者提出的路徑規劃方法主要分為兩類:基于采樣的路徑規劃和基于搜索的路徑規劃[1]。 快速擴展隨機樹(rapidly-exploring random tree，RRT)[2]算法為基于采樣的路徑規劃方法的代表性算法之一，已經被廣泛應用于工程實踐中，例如機械臂運動規劃[3]、機器人路徑規劃[4-6]以及無人機航跡規劃[7-9]。 RRT 算法無需對空間進行預處理即可進行隨機采樣，通過對采樣點進行處理，最終得到一條無碰撞的路徑。 RRT 算法具有概率完備性、搜索范圍廣等優點，但其生成的路徑并不是最優的。 針對這一問題，文獻[10]提出了RRT*算法，通過對擴展節點周邊一定范圍內的節點進行選擇，找到合適的父節點，從而規劃出漸進最優的路徑。 文獻[11]提出了RRT-connect方法，在起始點和終點同時生成兩棵擴展隨機樹進行路徑的尋找，加快了路徑尋找速度。 文獻[12]在RRT 擴展過程中通過調節采樣區范圍提高算法效率。

由此可見，改進的RRT 算法主要是對搜索策略和采樣區域選擇進行優化。 本文提出一種從預生成路徑本身出發，通過構建重規劃起止點進行RRT 規劃的方法。 在RRT 算法中通過縮小采樣區域，從而減少采樣次數，并且引入目標偏向尋優策略，使得采樣點具有向目標點偏轉的概率。 在平滑度優化策略中，去除冗余初始航跡點的同時與四旋翼無人機性能約束相結合，利用B 樣條曲線對航跡點處理，從而獲得更加符合四旋翼無人機特性的可行路徑。

1 基本RRT 算法

在路徑規劃過程中，將四旋翼無人機視為質點，RRT 算法從起始點出發，在空間中隨機生成采樣點，當采樣到目標點或其附近區域時停止，即生成一顆同時包含起始點和目標點的隨機樹，在樹中可以找到一條從起始點出發由樹的節點構成最終到達目標點的路徑。

路徑搜索過程如圖1 所示，圖中Kstart表示起始點，Kgoal表示目標點，正方形和圓形表示障礙物。 起始點Kstart可作為隨機樹的根節點，RRT 算法首先在非障礙物區域內隨機生成一個采樣點Krand作為目標點，在找到距離Krand最近的樹擴展節點Knear，以Knear為基準點在其與Krand的連線上朝著Krand方向按照預定步長S生成新的節點Knew，如果在預定步長內與障礙物發生碰撞則放棄Knew再重新生成Krand，如果在預定步長內無障礙物碰撞則保留Knew。 重復上述步驟，經過多次迭代，當Knew在目標點Kgoal附近區域時則完成隨機樹的構建，即找到了一條由起始點抵達目標點的可行路徑。

圖1 路徑搜索過程Fig.1 The path search process

2 改進RRT 算法的路徑搜尋方法

2.1 配置空間等效問題

四旋翼無人機在飛行過程中可能存在不同類型的障礙物，例如電線桿、樹木、指示牌等，但障礙物的細節對無人機的路徑規劃意義不大，因此在配置空間中需要對不同類型的障礙物進行統一等效處理。

假設對于任何障礙物，均可找到一個質量分布均勻的長方體將其完整包圍[13]。 本文對于空中障礙物將其等效為圓球，假設圓球的圓心為長方體質心，圓球半徑Rq由公式(1)求得。 對于地面障礙物，本文將其等效為圓柱，假設圓柱體底面圓心為長方體底面中心點，底面半徑Rc和圓柱高度Zc由公式(2)求得。 圖2 為本文障礙物等效示意圖。

圖2 障礙物等效示意圖Fig.2 Equivalent schematic diagram of obstacles

式中:Z為四旋翼無人機軸距；Tq為長方體對角線長度；Ty為長方體底面對角線長度；Hy為長方體的高。

2.2 四旋翼無人機性能約束

在空中飛行的四旋翼無人機不僅受到環境因素影響，還需考慮其自身性能約束，避免無人機發生劇烈運動，導致失控。 因此，在路徑規劃時需綜合考慮航跡距離、偏航角和俯仰角等性能約束條件。

2.2.1 最遠航跡約束條件

四旋翼無人機在飛行過程中由于受到自身動力限制并不能無限制距離飛行，因此在軌跡規劃時應該加入航跡約束。 無人機最遠飛行航跡由無人機搭載的電源和通信能力決定，路徑重規劃得出的軌跡由n個航跡點組成，其軌跡長度總和應小于最遠飛行航跡距離。 航跡約束條件可表示為

式中:i＝2，3，…，n；Dis(ki-1，ki)為相鄰航跡點之間的歐幾里得距離；Smax為無人機最遠飛行航跡距離。

2.2.2 最短航跡約束條件

四旋翼無人機在飛行時要盡量以勻速狀態保持直線飛行或者以恒定的航向角完成轉彎動作，頻繁的姿態調整對無人機控制系統有著較高的要求，電機需要不斷調整旋轉速度，會造成動力源消耗過多。 因此需要進行最短航跡約束，即路徑重規劃的過程中期望每個航跡點之間的距離大于預設的最短航跡距離Smin，如果產生n個航跡點，則最短航跡約束可以表示為

2.2.3 最大偏航角約束

偏航角為無人機實際航向與計劃航向之間的夾角，如果在飛行過程中偏航角過大可能會造成無人機遠離預設航向，導致飛行任務失敗，甚至失控，因此，需要在路徑規劃過程中設定最大偏航角限制。 在配置空間中，假設生成n個航跡點，對于除第n點外的其他航跡點ki(i＝2，3，…，n-1)為原點建立三維坐標系，在此坐標系中標定下一個航跡點ki+1的位置，這兩個航跡點之間的連線在xkiy平面的投影線段與x軸之間的夾角ρ應比設定的四旋翼無人機最大偏航角ρmax小，如圖3 所示，ki與ki+1為相鄰航跡點，最大偏航角約束可表示為

圖3 偏航角示意圖Fig.3 Schematic diagram of yaw angle

式中x、y為航跡點坐標值。

2.2.4 最大俯仰角約束

俯仰角為無人機機頭方向的機體軸與水平面之間的夾角，俯仰角過大會極大降低飛行過程中的穩定性，甚至造成無人機失控，因此需要設定最大俯仰角對其進行約束。 最大俯仰角限制與最大偏航角類似，如圖4 所示，在xkiz平面的投影線段與x軸之間的夾角γ應比設定的四旋翼無人機最大俯仰角γmax小，最大俯仰角約束可表示為

圖4 俯仰角示意圖Fig.4 Schematic diagram of pitch angle

式中x、z為航跡點坐標值。

2.3 路徑重規劃策略

2.3.1 構建重規劃起止點

在配置空間中，設置起始點為Ka(xa，ya，za)，目標點為Kb(xb，yb，zb)，先從xa指向xb預生成一條忽略障礙物的直線路徑，然后對于這條直線進行離散化處理，生成一組相鄰數組為單位步長的三列矩陣。 在路徑規劃過程生成的點屬于直角坐標系，為了確定配置空間中點、線之間的關系以及各種夾角的角度，這時需要將其轉換為球坐標系。 已知直角坐標系下Ka和Kb的坐標點，需要根據公式(7)將其轉換為球坐標系，坐標系示意圖如圖5 所示，圖中以Ka為原點建立三維坐標系，并在坐標系中標定Kb的位置，本文中其他需要進行坐標系轉換的坐標點均按此方法建立坐標系。

圖5 坐標系示意圖Fig.5 Schematic diagram of coordinate system

式中:l為Ka(xa，ya，za)和Kb(xb，yb，zb)之間的歐幾里得距離；θ為Ka和Kb的連線與z軸夾角；ρ為Ka和Kb的連線與x軸夾角。

式中:i＝1，2，3，…，k；x1、y1、z1為需要離散化處理線段的起始點坐標值。

依次對矩陣Γ每一行按照公式(9)計算歐幾里得距離進行檢測以判斷是否在障礙物內，并將包含在障礙物內的元素存入3 列矩陣Λ。

式中:j＝1，2，3，…，k；Γj1，Γj2，Γj3分別為矩陣Γ每行的元素。

以矩陣Λ中間行數元素λmid為起點，平行于配置空間中的底面，做的垂線γ，并對γ根據公式(8)以單位步長進行離散化，根據公式(9)找到包含在障礙物內的元素，分別找到最貼近障礙物兩側邊界的元素Qr與。 根據公式(10)計算與矩陣Λ中間維數元素Λmid的歐幾里得距離，比較兩者長度，將距離近的邊界元素保存為Q。 以Λmid為起點，以Q為方向，在γ離散化后得到的點中找到遠離障礙物并在10 ～30 倍單位步長中的一個元素保存為Qmid。

矩陣Λ的首尾元素也存在于矩陣Γ，矩陣Γ相鄰元素距離為單位步長，以矩陣Λ首尾元素為起點向矩陣Γ首行和尾行兩個方向進行擴展，找到遠離障礙物并在20 到30 倍單位步長之間的兩個元素，距離起始點Ka近的元素保存為重規劃起點Qstart，另一個元素保存為重規劃終止點Qgoal，如圖6 所示。

圖6 重規劃示意圖Fig.6 Schematic diagram of reconstruction

使用RRT 算法進行路徑搜索，首先搜索Qstart到Qmid路徑，再搜索Qmid到Qgoal的路徑。

2.3.2 RRT 算法采樣點選取改進

在配置空間中，基本RRT 算法尋找Ka和Kb之間路徑時由于采樣區域過大，從而造成采樣次數過多使得隨機擴展樹分支多。 本文在路徑重規劃過程中，通過重新選取RRT 算法起止點，改進采樣區域選擇范圍，降低采樣次數，從而減少隨機樹的分支數量。

本文在采樣時引入基于概率的目標偏向尋優策略[14]，如公式(11)所示，設置一個小于1 的目標偏置閾值Pth。P為在0 ～1 范圍由均勻分布獲得的一個概率值，當P＜Pth時，設置目標點為隨機點，這樣使得隨機樹具有向目標點Kgoal方向擴展的可能，當P＞Pth時，則使用sample()函數生成采樣點。

基本RRT 算法采樣時區域范圍為Ka和Kb為頂點構成的三維空間，本文中將采樣區域縮小為Qstart和Qgoal為頂點構成的三維空間，如圖7 所示。 新的采樣區域范圍由公式(12)計算得出。

圖7 采樣區域范圍Fig.7 Sampling area range

式中:xstart、ystart、zstart為Qstart的三維坐標；xgoal、ygoal、zgoal為Qgoal的三維坐標；s'x、s'y、s'z為x、y、z三軸上的長度。

sample()函數生成的采樣點(xstart、ystart、zstart)為Qstart，由公式(13)計算得出。

2.4 平滑度優化策略

由于RRT 算法隨機性強從而導致路徑重規劃后得到的初始軌跡通常不平滑、存在過多的轉折。 這些軌跡由初始航跡點連接得到，為了提高路徑平滑度，滿足四旋翼無人機實際飛行需求，需要對初始航跡點進行篩選從而得出合適的航跡點，然后利用B 樣條對其進行平滑處理。 平滑度優化策略總流程如圖8 所示。

圖8 平滑度優化策略流程Fig.8 Smoothness optimization strategy process

航跡點篩選是將第一個初始航跡點設置為起始點，依次檢測后續航跡點與起始點連線之間是否有障礙物，如果連線不經過障礙物則將其視為冗余點刪除并檢測下一個航跡點，如果有障礙物則為被保留航跡點并將其設置為新的起始點，再重復上述操作，直至檢測到最后一個初始航跡點。 為了便于B 樣條進行優化，使得路徑更具有可行性，會適當保留個別冗余點，保留原則為對被保留航跡點之前第四個至第九個冗余點中隨機抽取一個，如果被保留航跡點之前冗余點數量不足四個則不保留。 在得到航跡點后對航跡點進行安全性檢測，即判斷航跡點是否滿足四旋翼無人機性能約束。

B 樣條是貝塞爾曲線一般化形式之一，具有保凸性、凸包性以及幾何不變性等多種優良特性[15]，常用于對于軌跡進行平滑處理[16]。 本文將剔除冗余點后的航跡點作為B 樣條控制點，從而生成平滑的軌跡。

3 數字仿真分析

為驗證上述算法的有效性，本文基于Matlab 2020a 編程，在中央處理器為Intel(R)Core(TM)i5 -8300H CPU ＠ 2. 30 GHz、顯卡為NVIDIA GeForce GTX 1050 Ti 的計算機上進行數字仿真實驗。

為了驗證本文算法的性能，與RRT 算法進行了對比仿真實驗，且選取的概率目標偏向巡游策略相同。 在仿真實驗過程中，仿真環境設置為1 000 ×1 000 ×300 區域，設置起始點Ka(0，0，0)、目標點Kb(900，900，200)，設置6 個經過等效后的障礙物，其相關信息如表1 所示，實驗環境如圖9 所示。 RRT 算法未添加無人機約束，本文算法最短航跡約束條件設置為Smin＝1(無量綱)，將最大偏航角以及最大俯仰角設置為60°，即ρmax＝γmax＝tan 60° ＝1.732。

表1 等效障礙物信息Table 1 Parameters of equivalent obstacles

圖9 仿真環境Fig.9 Simulation environment

RRT 算法實驗結果如圖10 所示，本文算法實驗結果如圖11 所示，圖12 表示本文算法的部分重規劃結果。 由圖10 ～12 可知，重規劃過程中改進RRT 算法的隨機樹的分支相對于RRT 算法顯著減少，經過平滑度優化策略處理過的軌跡中的航跡點冗余點更少，相對于重規劃RRT 算法生成的原軌跡更加順滑，軌跡轉彎次數更少，更加符合四旋翼無人機的實際飛行需求。

圖10 RRT 算法xy 平面視圖Fig.10 xy-plane view of RRT algorithm

圖11 本文算法xy 平面視圖Fig.11 xy-plane view of algorithm in this paper

圖12 本文算法重規劃xy 平面視圖Fig.12 The reprogrammed xy-plane views of algorithm in this paper

為驗證本文提出算法的魯棒性，實驗分為2組進行，每組運行RRT 算法和本文算法各20 次，分別統計平均采樣次數、程序平均運行時間和Ka至Kb平均航跡距離數據，結果如表2 所示。

表2 實驗結果對比Table 2 Comparison of experimental results

實驗結果表明:本文算法相對于RRT 算法程序有效降低了采樣次數，平均采樣次數僅為RRT算法的24.60%；平均運行時間更少僅為原算法的29.05%。 表2 中航跡距離為歐幾里得距離，可以看出本文算法從起始點Ka到目標點Kb的航跡距離比RRT 算法的結果有所減少，為RRT 算法的94.60%。

4 結論

本文提出了一種改進RRT 算法的四旋翼無人機路徑規劃方法，旨在解決RRT 算法隨機性強、采樣次數過多以及生成軌跡曲折的問題。 在配置空間中對障礙物進行等效處理，忽略障礙物細節，降低了算法運行環境的復雜度。 在路徑重規劃過程中，通過重新構建采樣區域范圍，有效降低了RRT 算法的采樣次數，引入基于概率的目標偏向尋優策略，使采樣點以一定概率向重規劃目標點方向進行擴展。 在平滑度優化策略中，選擇性去除初始航跡中的冗余點并兼顧無人機性能約束，通過B 樣條曲線產生平滑路徑，使得生成的路徑更加貼合無人機實際需求。