馬 蓉

(航空工業(yè)西安飛行自動(dòng)控制研究所 飛行器控制一體化技術(shù)國(guó)家重點(diǎn)實(shí)驗(yàn)室，西安 710076)

0 引言

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

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

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

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

1 無(wú)人機(jī)航路規(guī)劃與跟蹤方法設(shè)計(jì)

1.1 經(jīng)典RRT算法

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

算法2 RRT算法

1.2 基于改進(jìn)RRT算法的無(wú)人機(jī)航路規(guī)劃方法設(shè)計(jì)

在結(jié)合改進(jìn)的RRT算法進(jìn)行航路規(guī)劃時(shí)，規(guī)劃出的航路為每單位步長(zhǎng)規(guī)劃一個(gè)航路點(diǎn)，航路點(diǎn)既需位于無(wú)碰撞區(qū)域，又要滿足無(wú)人機(jī)的動(dòng)力學(xué)約束[12]。本文以固定翼無(wú)人機(jī)為主要研究對(duì)象，對(duì)其航路規(guī)劃算法進(jìn)行設(shè)計(jì)。

如果已知轉(zhuǎn)彎時(shí)地速的水平分量，則轉(zhuǎn)彎的構(gòu)造如下

(1)

其中，Rmin為固定翼無(wú)人機(jī)的最小轉(zhuǎn)彎半徑，VH為轉(zhuǎn)彎時(shí)地速的水平分量，φmax為最大滾轉(zhuǎn)角。

Δφ=dφ*Tstep

(2)

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

三維環(huán)境中的航路規(guī)劃，還需考慮無(wú)人機(jī)的縱向運(yùn)動(dòng)軌跡，與橫側(cè)向動(dòng)力學(xué)約束條件類似，需對(duì)俯仰角變化率進(jìn)行約束，以免失速[13]。

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

圖1 三維空間固定翼無(wú)人機(jī)動(dòng)力學(xué)約束Fig.1 The scheme of dynamic restriction for fixed-wing UAVs in 3D space

設(shè)定固定翼無(wú)人機(jī)在其動(dòng)力學(xué)約束條件下可飛速度向量有9個(gè)，得到pi，表示如下

(3)

(4)

(5)

(6)

(7)

仿真結(jié)果如圖2所示。

圖2 固定翼無(wú)人機(jī)航路規(guī)劃仿真結(jié)果Fig.2 The simulation result of path planning for fixed-wing UAVs

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

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

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

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

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

1.4 航路跟蹤控制律設(shè)計(jì)

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

圖4 航路跟蹤控制律結(jié)構(gòu)Fig.4 Trajectory tracking control law

由試驗(yàn)可知，在無(wú)規(guī)劃的滾轉(zhuǎn)角指令輸入時(shí)，直接由側(cè)向偏離的控制律跟蹤航路點(diǎn)的坐標(biāo)位置不能很好地跟蹤航路。

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

(a)無(wú)滾轉(zhuǎn)指令輸入的跟蹤結(jié)果

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

2 基于兩種無(wú)人機(jī)平臺(tái)的試飛驗(yàn)證

2.1 試飛平臺(tái)搭建

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

(a)小型固定翼無(wú)人機(jī)平臺(tái)

(b)旋翼無(wú)人機(jī)平臺(tái)圖6 試飛驗(yàn)證平臺(tái)Fig.6 Platforms for flight tests

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

2.2 試飛結(jié)果與分析

基于固定翼平臺(tái)的自主避障飛行試驗(yàn)中，2m翼展小型固定翼無(wú)人機(jī)搭載激光雷達(dá)全自動(dòng)飛行，飛行速度為8m/s，如圖7所示。

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

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

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

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

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

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

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

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

對(duì)于固定翼無(wú)人機(jī)平臺(tái)，規(guī)劃時(shí)長(zhǎng)應(yīng)小于無(wú)人機(jī)到達(dá)下一個(gè)航路點(diǎn)的時(shí)長(zhǎng)，否則即使航路規(guī)劃成功也無(wú)法進(jìn)行航路跟蹤。演示驗(yàn)證中，選取了0.3s規(guī)劃時(shí)間步長(zhǎng)，最長(zhǎng)規(guī)劃用時(shí)符合規(guī)劃時(shí)長(zhǎng)要求，滿足自主航路規(guī)劃的實(shí)時(shí)性要求。

3 結(jié)論

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

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