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

馬 蓉

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

0 引言

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

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

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

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

1 無人機航路規(guī)劃與跟蹤方法設計

1.1 經典RRT算法

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

算法2 RRT算法

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

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

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

(1)

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

Δφ=dφ*Tstep

(2)

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

三維環(huán)境中的航路規(guī)劃，還需考慮無人機的縱向運動軌跡，與橫側向動力學約束條件類似，需對俯仰角變化率進行約束，以免失速[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 固定翼無人機航路規(guī)劃仿真結果Fig.2 The simulation result of path planning for fixed-wing UAVs

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

1.3 局部航路動態(tài)優(yōu)化策略

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

圖3 動態(tài)環(huán)境下的航路重規(guī)劃Fig.3 Path re-planning in dynamic environment

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

1.4 航路跟蹤控制律設計

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

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

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

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

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

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

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

2.1 試飛平臺搭建

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

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

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

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

2.2 試飛結果與分析

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

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

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

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

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

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

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

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

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

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

3 結論

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

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