基于改進RRT算法的無人機航路規劃與跟蹤方法研究

馬 蓉

(航空工業西安飛行自動控制研究所 飛行器控制一體化技術國家重點實驗室，西安 710076)

0 引言

目前，無人機在軍用及民用領域的應用呈井噴式增長，可以預見，未來的空域環境趨于復雜化。面對日趨復雜的空域環境，無人機的自主能力等級將極大地影響平臺的生存概率及執行任務種類的數目。 例如無人機以較低的高度在城市或者森林中穿梭時，需要具備較強的自主規避能力，航路規劃與跟蹤方法即為其關鍵技術之一。

無人機航路規劃的目的是根據任務要求、環境特征和飛行平臺的動力學約束條件規劃出一條有效的航路[1]。目前主流的航路規劃方法可以分為三類：以A*為代表[2]的基于幾何模型搜索的方法、以人工勢場法[3]為代表的虛擬勢場方法，以及近幾年興起的基于人工智能的方法包括蟻群算法[4-5]、遺傳算法[6]、粒子群算法[7]等。其中A*算法應用較為普及，其在二維平面內可靠性高、運算速度快，應用于無人車與地面機器人效果較好，但是面對三維空間時，該算法的計算量成倍增長，因而無法保證實時性[8]。人工勢場的規劃方法的實時性較高，但是該算法難以結合運動體的運動學和動力學微分約束，因而無法約束無人機在沿所規劃航路快速飛行時的偏差，不能保證無人機不與障礙物發生碰撞[9]。基于人工智能的方法同樣存在計算量大、運行時間長的問題[10]。無人機在復雜并且存在動態障礙物的環境中飛行時，對航路規劃及跟蹤方法具有較高的實時性與可靠性要求，因此亟需一種能夠快速規劃路徑并精確跟蹤的方法。

本文的航路規劃算法以快速擴展隨機樹 (Rapidly-exploring Random Tree，RRT)算法[11]為基礎，較其他方法在實時性和處理環境不確定性方面具有較強優勢。由于算法規劃出的航路對于具有較多動力學約束條件的無人機無法直接使用，需要對其進行二次優化，效率較低。本文采取了一種對RRT算法的規劃節點進行動力學約束的方法，該方法在航路規劃的過程中即對航路點進行動力學約束，無需二次優化，提高了算法的實時性。同時，由于RRT算法是一種全局規劃算法，面對動態的任務場景時，需要不停地更新整條航路，不僅增加了計算負荷而且不利于航路跟蹤的穩定性。本文設計了局部航路動態優化策略來提高算法效率。此外，為了提高航路跟蹤的準確性，在航路跟蹤控制律中加入了姿態控制指令，對傳統的跟蹤控制方法進行了改進。

最后，本文通過自主避障飛行演示驗證試驗，證明了該算法的有效性。

1 無人機航路規劃與跟蹤方法設計

1.1 經典RRT算法

RRT算法是一種基于隨機采樣的樹結構搜索算法[11]，以狀態空間中給定的起點xinitial出發，通過在狀態空間中隨機采樣引導搜索樹生長。當樹的節點進入目標區域χgoal時算法結束，然后回溯到根節點即可得到所規劃航路。算法流程如算法2所示。

算法2 RRT算法

1.2 基于改進RRT算法的無人機航路規劃方法設計

在結合改進的RRT算法進行航路規劃時，規劃出的航路為每單位步長規劃一個航路點，航路點既需位于無碰撞區域，又要滿足無人機的動力學約束[12]。本文以固定翼無人機為主要研究對象，對其航路規劃算法進行設計。

如果已知轉彎時地速的水平分量，則轉彎的構造如下

(1)

其中，Rmin為固定翼無人機的最小轉彎半徑，VH為轉彎時地速的水平分量，φmax為最大滾轉角。

Δφ=dφ*Tstep

(2)

其中，dφ為偏航角速率，Tstep為規劃時間步長。c為pi-1到pi的弦長，由于c值較小，所以c≈l=VH*Tstep。

三維環境中的航路規劃，還需考慮無人機的縱向運動軌跡，與橫側向動力學約束條件類似，需對俯仰角變化率進行約束，以免失速[13]。

如圖1所示，三維空間中，已知當前位置信息為pi-1，俯仰角和航向角分別為θ0和φ0。

圖1 三維空間固定翼無人機動力學約束Fig.1 The scheme of dynamic restriction for fixed-wing UAVs in 3D space

設定固定翼無人機在其動力學約束條件下可飛速度向量有9個，得到pi，表示如下

(3)

(4)

(5)

(6)

(7)

仿真結果如圖2所示。

圖2 固定翼無人機航路規劃仿真結果Fig.2 The simulation result of path planning for fixed-wing UAVs

由圖2可以看出，本文算法規劃出的航路既能夠避開障礙區域又滿足固定翼無人機的動力學約束。

1.3 局部航路動態優化策略

RRT航路規劃方法是一種全局規劃算法，在動態環境中，為了保持實時性，需要每一拍都計算新的航路(圖3)，導致航路點不停變化，不利于進行航路跟蹤，而且計算機負擔過重[14]。

圖3 動態環境下的航路重規劃Fig.3 Path re-planning in dynamic environment

為了使航路保持穩定以及減少計算負荷，本文采取了局部航路動態優化策略，即首先檢查上一拍規劃出的航路是否依舊可行，計算每個航路點是否處于安全區域。如果可行，保留原有航路并繼續跟蹤；如果有航點處于碰撞區，即重規劃該航路點及其之后的航點并更新航路。如未能重規劃成功，說明該航點脫離上一個航點的規劃約束范圍，追溯回之前的航點進行重規劃，直至規劃出新的可行航路。

1.4 航路跟蹤控制律設計

為實現快速準確的航路跟蹤，本文設計了航路跟蹤控制律模塊[15]。航路規劃算法規劃出的每個航路點包含信息有[position_nyaw_nroll_n],其中position_n為航路點位置信息，yaw_n為航向角信息，roll_n為滾轉角指令信息，如圖4所示。

圖4 航路跟蹤控制律結構Fig.4 Trajectory tracking control law

由試驗可知，在無規劃的滾轉角指令輸入時，直接由側向偏離的控制律跟蹤航路點的坐標位置不能很好地跟蹤航路。

如圖5(a)所示，紅色軌跡為規劃航路點，藍色軌跡為實際飛行航路。其原因是側向糾偏控制律需要在誤差已產生的情況下才能進行糾偏，所以跟蹤航路存在一定延遲。增加了滾轉角指令輸入后，跟蹤控制律能夠較好地跟蹤規劃航路。由試驗數據，如圖5(b)可知，其側偏距跟蹤誤差可以保持在2.5m以內，能夠滿足本試驗中對小型固定翼無人機避障的安全性要求。

(a)無滾轉指令輸入的跟蹤結果

(b)加入滾轉指令輸入的跟蹤結果圖5 航路跟蹤仿真結果Fig.5 Simulation results of trajectory tracking

2 基于兩種無人機平臺的試飛驗證

2.1 試飛平臺搭建

為了驗證本文提出的航路規劃方法對不同的無人機平臺及環境均具有良好的適用性。本文搭建了2款小型無人機驗證平臺，分別為固定翼和四旋翼，如圖6所示。

(a)小型固定翼無人機平臺

(b)旋翼無人機平臺圖6 試飛驗證平臺Fig.6 Platforms for flight tests

2款無人機平臺均配置了用于環境感知的傳感器。其中固定翼無人機配置了Hokuyo激光雷達傳感器，探測距離為30m，精度為0.03m；小型旋翼無人機配置了同款激光雷達及單目視覺傳感器，單目視覺傳感器為激光雷達獲得的環境信息進行補充，主要為環境中的紋理信息。利用環境感知傳感器，無人機平臺在飛行的過程中，可獲得環境中的障礙物信息并進行環境建模，為無人機實現自主避障奠定基礎。

2.2 試飛結果與分析

基于固定翼平臺的自主避障飛行試驗中，2m翼展小型固定翼無人機搭載激光雷達全自動飛行，飛行速度為8m/s，如圖7所示。

圖7 基于固定翼無人機的自主避障視頻截圖Fig.7 Obstacle avoidance by fixed-wing UAV

固定翼無人機自動起飛后盤旋一周對準跑道，以4m相對高度低空通場，依靠機載激光雷達感知跑道上樹立的2個10m高充氣立柱，先后完成2次在線運動規劃與航路跟蹤，成功繞開障礙物。

由圖8可以看出，航路跟蹤側偏距小于2.5m，滿足精度要求。

圖8 航跡跟蹤側偏距Fig.8 Lateral deviation of trajectory tracking

基于四旋翼平臺的自主避障飛行試驗中，旋翼無人機巡航速度2m/s，需要在樹林中執行任務，對無人機的自主避障性能具有更高要求，如圖9所示。無人機在飛行至規定的目標點的過程中，順利完成自主避障，跟蹤誤差不超過0.3m，滿足小型旋翼無人機自主避障需求。

圖9 基于四旋翼平臺的自主避障視頻截圖Fig.9 Obstacle avoidance by quadrotors UAV

本文根據不同步長，對算法的規劃用時進行了測試，每個步長下測試10次，取平均用時和最長用時。 由表1可知，規劃時間隨著規劃步長減少，但是由于規劃步長過大會導致規劃出的航路點越過障礙物，因此，需要根據任務場景中障礙物的具體情況，設定合適的規劃步長。

表1 規劃時間步長與規劃時間Tab.1 Planning time step and planning time

對于固定翼無人機平臺，規劃時長應小于無人機到達下一個航路點的時長，否則即使航路規劃成功也無法進行航路跟蹤。演示驗證中，選取了0.3s規劃時間步長，最長規劃用時符合規劃時長要求，滿足自主航路規劃的實時性要求。

3 結論

本文針對無人機復雜任務場景中自主航路規劃能力的需求，對經典RRT算法進行了改進。對規劃航路點進行了無人機飛行動力學約束，無需二次優化航路，提高了算法在三維空間航路規劃的實時性。設計了局部航路動態優化策略，減少了算法在動態場景中的計算負荷，并且使航路點趨于穩定。針對傳統的航路跟蹤控制律避障時跟蹤誤差較大的問題，通過將規劃算法得出的姿態指令引入姿態控制回路的方式，提高了航路跟蹤控制精度。利用固定翼和旋翼兩種無人機平臺在多種任務場景下的試飛驗證，證明了本文提出的自主航路規劃方法在工程應用中的可行性與有效性，說明了該航路規劃方法能夠有效提高無人機的自主能力。

由于無人機在自主規避的過程中，需要準確的環境模型，本文目前采用的環境模型較為簡單，不適用于更復雜的環境。在后續的研究中，將會側重于環境感知與建模方面的研究，以提升無人機自主規避技術在復雜環境中應用的效果。