呂智虎,梁曉龍,任寶祥,李 哲,張佳強(qiáng),齊 鐸,侯岳奇
(1.空軍工程大學(xué)空管領(lǐng)航學(xué)院,西安,710051; 2.陜西省電子信息系統(tǒng)綜合集成重點實驗室,西安,710051)
無人機(jī)具有隱蔽性好、機(jī)動性強(qiáng)、成本較低等特點,隨著無人機(jī)應(yīng)用領(lǐng)域不斷拓展,所處的飛行環(huán)境也更加復(fù)雜,提高無人機(jī)的自主避障能力成為確保任務(wù)完成的必要條件[1]。
目前避障算法主要包括勢場法[2]、基于圖論的算法[3]和智能優(yōu)化算法[4]。基于圖論的避障算法主要有A*算法[5]、D*算法[6]、Voronoi圖法[7]等。智能優(yōu)化算法主要包括遺傳算法[8]、蟻群算法[9]、人工蜂群算法[10]、粒子群算法[11]等。當(dāng)前的無人機(jī)避障方法大部分是在障礙物信息已知的條件下進(jìn)行全局路徑預(yù)規(guī)劃,對有碰撞風(fēng)險的路徑進(jìn)行重規(guī)劃從而實現(xiàn)避障要求。而在環(huán)境中障礙物信息未知的情況下,無人機(jī)難以進(jìn)行全局路徑規(guī)劃,需要無人機(jī)能夠?qū)崟r探測并規(guī)劃避障路徑。
針對未知環(huán)境中的無人機(jī)避障問題,文獻(xiàn)[12]提出了一種改進(jìn)人工勢場路徑規(guī)劃算法,實現(xiàn)了未知環(huán)境中對靜態(tài)、動態(tài)障礙物的規(guī)避,但其避障路徑中存在冗余路徑。文獻(xiàn)[13]提出一種多層雙向A*算法,使智能體可以在復(fù)雜環(huán)境中實時規(guī)避突發(fā)障礙,但該算法僅適用于少量存在未知障礙物的情況。文獻(xiàn)[14]將模糊神經(jīng)網(wǎng)絡(luò)應(yīng)用于無人機(jī)電力巡檢,無人機(jī)能夠沿預(yù)設(shè)路徑飛行并規(guī)避未知障礙物,但沒有利用無人機(jī)運(yùn)動方向與障礙物的角度信息,增加避障路徑。
模糊神經(jīng)網(wǎng)絡(luò)能夠結(jié)合已有的避障規(guī)則與障礙物特征,通過訓(xùn)練優(yōu)化模糊控制器的參數(shù),使無人機(jī)在障礙物信息未知的環(huán)境中依然能夠?qū)崿F(xiàn)避障[15]。
本文首先對模糊控制器的輸入變量、輸出變量以及初始隸屬度函數(shù)進(jìn)行設(shè)計,優(yōu)化了模糊控制器的輸入變量;其次利用模糊控制器得到無人機(jī)避障的理想路徑,從該路徑中提取避障過程中無人機(jī)運(yùn)動狀態(tài)、與障礙物位置關(guān)系的數(shù)據(jù)生成訓(xùn)練數(shù)據(jù)集,用于訓(xùn)練模糊神經(jīng)網(wǎng)絡(luò);最后將訓(xùn)練后的模糊神經(jīng)網(wǎng)絡(luò)在新的未知環(huán)境中進(jìn)行測試驗證。
在障礙物信息未知的室內(nèi)飛行區(qū)域中,給定起始點O與目標(biāo)點E,在飛行過程中無人機(jī)能夠主動探測環(huán)境中障礙物信息,并對障礙物進(jìn)行實時規(guī)避,最終安全抵達(dá)目標(biāo)點,如圖1所示。由于室內(nèi)環(huán)境較為狹窄,因此選用低速微型無人機(jī)。
圖1 避障環(huán)境模型
1.2.1 傳感器模型
激光雷達(dá)能夠測得目標(biāo)距離、方位甚至形狀等參數(shù),具有測量精度高、探測范圍大以及結(jié)構(gòu)簡單等優(yōu)點,在避障和建圖中較為常用[16]。目前激光雷達(dá)主要有兩種測距原理:三角測距原理與TOF測距原理[17],其中,TOF雷達(dá)可以測量的距離更遠(yuǎn),并且可以在長距離范圍內(nèi)保持較高精度[18]。
(1)
則無人機(jī)到第i個障礙點的距離di為:
(2)
式中:c表示電磁波傳播速度。
(3)
1.2.2 無人機(jī)運(yùn)動模型
給定無人機(jī)某一時刻的運(yùn)動速度大小改變量與航向角改變量,通過無人機(jī)運(yùn)動方程來計算無人機(jī)的下一時刻位置,實現(xiàn)無人機(jī)的運(yùn)動控制。
設(shè)定無人機(jī)的控制量更新周期為Ts,通過式(4)便可計算出其下一航跡點位置。
(4)
式中:v(t)為t時刻無人機(jī)運(yùn)動速度大??;α(t)為t時刻無人機(jī)航向角。其更新公式為:
(5)
式中:Δv及Δα分別為t時刻的速度改變量及航向角改變量,單位分別為m/s及rad/s。
1.2.3 環(huán)境模型
文獻(xiàn)[19]將障礙物劃分為平面類、棱角類、弧形類3類,成功實現(xiàn)了障礙物的形狀識別。文獻(xiàn)[20]將不規(guī)則障礙物進(jìn)行分割或者補(bǔ)全為規(guī)則的四邊形、圓形等凸多邊形,再利用幾何法計算無人機(jī)登陸點,最后選取最優(yōu)路徑實現(xiàn)避障。
本文將障礙物簡化為3類,分別是四邊形、三角形、圓形,其他復(fù)雜障礙物均可以由這3類組合而成。為模擬未知環(huán)境,無人機(jī)在飛行過程中只能獲取探測半徑以內(nèi)的障礙物信息。
無人機(jī)的實時避障流程如圖2所示。
圖2 無人機(jī)避障流程
如圖3所示,設(shè)無人機(jī)自身位置與目標(biāo)點之間的距離為Df,無人機(jī)與目標(biāo)點最大許可距離為Da。將Da作為判斷無人機(jī)是否抵達(dá)目標(biāo)點的依據(jù),即當(dāng)無人機(jī)與目標(biāo)點的距離小于最大許可距離時,認(rèn)為無人機(jī)已經(jīng)到達(dá)目標(biāo)點。探測器獲取的附近障礙物的最小距離為Dmin,無人機(jī)探測半徑為Rd。
圖3 避障流程參數(shù)說明
Step1無人機(jī)開始飛行并檢測距離Df,如果Df>Da,即無人機(jī)與目標(biāo)點的距離大于最大許可距離,轉(zhuǎn)到Step2,否則轉(zhuǎn)到Step5;
Step2無人機(jī)實時探測障礙物,如果探測到障礙物,即Dmin Step3無人機(jī)獲取到障礙物距離與角度信息,在模糊神經(jīng)網(wǎng)絡(luò)控制器下進(jìn)行避障,轉(zhuǎn)到Step2; Step4無人機(jī)在目標(biāo)點的引力作用下,以當(dāng)前位置與目標(biāo)點連線為航向角飛行,逐漸增大速度,轉(zhuǎn)到Step1; Step5抵達(dá)目標(biāo)點,結(jié)束飛行。 為獲取模糊神經(jīng)網(wǎng)絡(luò)避障算法的訓(xùn)練數(shù)據(jù),首先需要設(shè)計能夠有效規(guī)避障礙物的Mamdani型模糊控制器,然后提取無人機(jī)在避障過程中的數(shù)據(jù),最后用于模糊神經(jīng)網(wǎng)絡(luò)的訓(xùn)練。 2.1.1 輸入變量定義 為了使障礙物信息得到充分利用,將模糊控制器的輸入變量定義為無人機(jī)到探測范圍內(nèi)障礙點的最小距離d和無人機(jī)當(dāng)前運(yùn)動方向與無人機(jī)與障礙點連線形成的等效夾角θ,如圖4所示。 圖4 模糊控制器輸入變量 O點表示無人機(jī)當(dāng)前位置,E表示目標(biāo)點,D1,D2,…,Dn表示在無人機(jī)探測范圍內(nèi)的障礙物邊界離散點,n為探測范圍內(nèi)的障礙點個數(shù),di為無人機(jī)到探測范圍內(nèi)第i個障礙點的距離,則模糊控制器第1個輸入變量d為: d=min{d1,d2,…,dn} (6) 將式(1)、式(2)代入式(6)得到d的計算公式: (7) (8) θi為無人機(jī)當(dāng)前運(yùn)動方向與無人機(jī)與第i個障礙點連線的夾角,其計算公式為: (9) 將式(3)、式(9)代入到式(8)得到θ計算公式: (10) 2.1.2 輸出變量定義 本文中無人機(jī)的運(yùn)動狀態(tài)由速度大小與航向角確定,輸出變量設(shè)計為運(yùn)動速度大小改變量Δv與航向角改變量Δα。 2.2.1 輸入變量隸屬度函數(shù) 對于輸入變量d,其論域是由無人機(jī)最大探測范圍所決定的。目前市面上的激光雷達(dá),例如HOKUYO公司的UST-05LX,SLAMTEC公司的RPLIDAR等,能夠很好地實現(xiàn)對15 m以內(nèi)障礙物的探測,因此確定d的模糊論域為[0,15],單位為m,模糊分割數(shù)為3,模糊集合為{近,中,遠(yuǎn)}={N,M,F(xiàn)}。其中“近”表示無人機(jī)與障礙物距離較近,“中”、“遠(yuǎn)”含義同理。其隸屬度函數(shù)采用高斯分布,如圖5所示。 圖5 輸入變量d隸屬度函數(shù) 對于輸入變量θ,其論域是由無人機(jī)運(yùn)動方向與無人機(jī)與障礙物相對位置關(guān)系共同確定,如圖6所示。 圖6 輸入變量θ范圍 只有θi為銳角時,才認(rèn)為此時的障礙點Di對無人機(jī)有碰撞威脅,否則忽略不滿足該條件的點。因此,θ模糊論域為[-π/2,π/2],單位為rad,模糊分割數(shù)為5,模糊集合為{左大,左下,中,右小,右大}={LB,LS,M,RS,RB},其中,“左大”表示障礙物主要集中分布在無人機(jī)運(yùn)動方向的左側(cè),且與無人機(jī)運(yùn)動方向的夾角較大。其他模糊語言的含義同理,其隸屬度函數(shù)定義如圖7。 圖7 輸入變量θ隸屬度函數(shù) 2.2.2 輸出變量隸屬度函數(shù) 為實現(xiàn)無人機(jī)更精確的控制,將輸出變量Δv、Δα的模糊分割數(shù)設(shè)計為5。同時為使無人機(jī)的運(yùn)動軌跡更為平滑,需要考慮其運(yùn)動學(xué)約束,本文限定了無人機(jī)的最大加速度與最大角速度。無人機(jī)的控制量更新周期Ts=0.25 s,速度改變量Δv論域范圍為[-0.5,0.5],即無人機(jī)加速度大小最大為2 m/s2,模糊集合為{驟減,減速,勻速,加速,驟增}={VS,S,M,F(xiàn),VF}。其隸屬度函數(shù)采用高斯分布,如圖8所示。 圖8 輸出變量Δv隸屬度函數(shù) 無人機(jī)最大角速度確定為1.57 rad/s,根據(jù)控制量新周期Ts=0.25 s,因此航向角改變量Δα論域范圍為[-0.393,0.393],模糊集合為{左大,左小,中等,右小,右大}={LB,LS,M,RS,RB}。其中“左大”表示航向角增加較大角度;即無人機(jī)向左轉(zhuǎn)較大角度;“中等”表示無人機(jī)航向角不發(fā)生明顯改變,其他模糊語言含義同理。其隸屬度函數(shù)如圖9所示。 圖9 輸出變量Δα隸屬度函數(shù) 2.2.3 模糊控制器規(guī)則設(shè)計 模糊控制器的規(guī)則設(shè)計,需要在已有經(jīng)驗的前提下,給定輸入、輸出變量的對應(yīng)關(guān)系,保證無人機(jī)能夠?qū)崿F(xiàn)躲避障礙物的功能。本文設(shè)計的兩個輸入變量d、θ的模糊分割數(shù)分別為3、5,因此共有15條規(guī)則,如表1所示。 表1 模糊控制器規(guī)則表 當(dāng)輸入變量d為“N”,θ為“LS”時,輸出變量Δv為“VS”,Δα為“TRB”,即當(dāng)無人機(jī)距離障礙物近,且當(dāng)前航向角方向與無人機(jī)與障礙物連線夾角較小時,需要減速,減小航向角,使無人機(jī)低速右轉(zhuǎn),從而安全規(guī)避障礙物。 2.2.4 輸出量控制曲面 圖10反映了模糊控制器的輸出變量Δv、Δα與輸入變量的變化關(guān)系。 圖10 速度與航向角控制曲面 由圖10可知,隨著d和|θ|的減小,輸出變量Δv減小。說明無人機(jī)距離障礙物越近,航向角越靠近障礙物,則越需要低速飛行。且Δv、Δα的變化較為平滑,保證了無人機(jī)飛行過程中能夠滿足運(yùn)動學(xué)約束。 模糊控制器采取固定的規(guī)則來實現(xiàn)無人機(jī)的避障要求,控制過程簡單,實時性較好。但是其控制器的參數(shù)與規(guī)則確定后無法更改,適應(yīng)性較差,當(dāng)面對不同的環(huán)境時,規(guī)劃路徑效果不理想。此外,由于固定規(guī)則的限制,面對“凹多邊形”障礙物時,容易進(jìn)入“陷阱”,無法實現(xiàn)避障要求。 模糊控制適應(yīng)性差,在障礙物分布差異較大的新環(huán)境中避障效果不穩(wěn)定。而模糊神經(jīng)網(wǎng)絡(luò)在通過訓(xùn)練后,能夠充分利用輸入信息,提取障礙物的深度特征,提高避障算法對環(huán)境的適應(yīng)性,使無人機(jī)在障礙物較為復(fù)雜的環(huán)境中也能夠完成避障任務(wù)。 在模糊系統(tǒng)中,主要有Mamdani模型和Takagi-Sugeno模型。Takagi-Sugeno型模糊推理計算簡單,有利于數(shù)學(xué)分析,且易于和PID控制方法以及優(yōu)化、自適應(yīng)方法相結(jié)合[21]。 圖11為Takagi-Sugeno模型的模糊神經(jīng)網(wǎng)絡(luò)結(jié)構(gòu)。該網(wǎng)絡(luò)由前件網(wǎng)絡(luò)和后件網(wǎng)絡(luò)兩部分組成[22]。前件網(wǎng)絡(luò)由4層組成,其作用分別為傳遞輸入,計算隸屬度、計算適應(yīng)度以及歸一化。后件網(wǎng)絡(luò)有yi個結(jié)構(gòu)相同的并列子網(wǎng)絡(luò)組成,每個子網(wǎng)絡(luò)生成一個輸出量。自網(wǎng)絡(luò)的第1層為輸入層,將輸入傳遞給第2層。第2層用于計算每一條規(guī)則的后件,即: 圖11 基于Takagi-Sugeno模型的模糊神經(jīng)網(wǎng)絡(luò)結(jié)構(gòu)圖 (11) 式中:j=1,2,…,m;i=1,2,…,r。 子網(wǎng)絡(luò)的第3層是計算系統(tǒng)的輸入,即: (12) yi是模糊神經(jīng)網(wǎng)絡(luò)的輸出,由各規(guī)則后件加權(quán)和計算,加權(quán)系數(shù)為各模糊規(guī)則經(jīng)歸一化的使用度,即前件網(wǎng)絡(luò)的輸出用于后件網(wǎng)絡(luò)第3層的連接權(quán)值。 本文利用MATLAB中的自適應(yīng)神經(jīng)模糊推理系統(tǒng)工具箱來完成模糊神經(jīng)網(wǎng)絡(luò)的設(shè)計與訓(xùn)練。首先無人機(jī)在模糊控制下進(jìn)行避障,提取避障路徑中的d、θ、Δv、Δα參數(shù)生成訓(xùn)練數(shù)據(jù)。然后將前文所設(shè)計模糊控制器中的輸入變量及隸屬度函數(shù)作為初始參數(shù)。由于d、θ模糊分割數(shù)分別為3、5,因此初始模糊神經(jīng)網(wǎng)絡(luò)同樣有15條規(guī)則,每條規(guī)則下都對應(yīng)一個輸出變量的隸屬度函數(shù)。MIMO的模糊規(guī)則可分解為多個MISO模糊規(guī)則,因此分別對Δv、Δα進(jìn)行訓(xùn)練。圖12為Δv、Δα的初始模糊神經(jīng)網(wǎng)絡(luò)結(jié)構(gòu)。 圖12 輸出變量Δv、Δα訓(xùn)練網(wǎng)絡(luò) 第1層為輸入變量層,將輸入變量值傳遞給網(wǎng)絡(luò);第2層為輸入變量隸屬函數(shù)層,其中定義了初始的隸屬度函數(shù);第3層為規(guī)則層,規(guī)則均采用and邏輯;第4層為輸出變量隸屬度函數(shù),每個隸屬度函數(shù)由輸入變量通過線性運(yùn)算得到;第5層為歸一化層,第6層為輸出變量。 之后將初始模糊神經(jīng)網(wǎng)絡(luò)、訓(xùn)練數(shù)據(jù)、測試數(shù)據(jù)導(dǎo)入至模糊推理系統(tǒng)編輯器,并設(shè)置訓(xùn)練算法、誤差精度以及訓(xùn)練次數(shù)。 本文選擇混合算法,訓(xùn)練誤差為0.003,訓(xùn)練次數(shù)為40。訓(xùn)練完成的輸入變量隸屬度函數(shù)如圖13所示。 圖13 輸入變量d、θ訓(xùn)練結(jié)果 與初始隸屬度函數(shù)(圖5~7)相比,該隸屬度函數(shù)的中心值與寬度值有了較大改變。最后將訓(xùn)練完畢的模糊神經(jīng)網(wǎng)絡(luò)在新的仿真環(huán)境中進(jìn)行測試,驗證其避障效果。 仿真環(huán)境為600 m×600 m的正方形,以原點(0,0)建立直角坐標(biāo)系,起始點坐標(biāo)(30,30),目標(biāo)點坐標(biāo)(500,500),無人機(jī)最大速度vmax=4 m/s,最小速度vmin=1 m/s,控制量更新周期Ts=0.25 s,最大角速度ωmax=1.57 rad/s,最大加速度amax=2 m/s2,最大探測半徑Rd=15 m。 圖14為模糊控制器在仿真環(huán)境下的避障效果,從圖中可以看出,在多次調(diào)整隸屬度函數(shù)和模糊規(guī)則后,控制器能夠?qū)崿F(xiàn)較好的避障效果,且運(yùn)動軌跡平滑,安全性較高。圖中標(biāo)注出了5個關(guān)鍵避障節(jié)點,其飛行仿真數(shù)據(jù)見表2。根據(jù)表中的數(shù)據(jù)與輸入、輸出變量隸屬度,可以確定每個節(jié)點無人機(jī)所采取的模糊規(guī)則,證實了模糊控制器實現(xiàn)避障的可行性。 圖14 模糊控制器避障結(jié)果 表2 關(guān)鍵節(jié)點飛行數(shù)據(jù) 但是模糊控制算法采用固定規(guī)則來實現(xiàn)避障,試湊法確定的模糊控制器參數(shù)無法實時變化。當(dāng)環(huán)境發(fā)生改變時,適應(yīng)性較差。此外,基于經(jīng)驗設(shè)計的規(guī)則只考慮了簡單的障礙物分布情況,當(dāng)環(huán)境中存在復(fù)雜“凹多邊形”區(qū)域,無人機(jī)便會進(jìn)入“陷阱”,無法實現(xiàn)避障要求。 圖15為兩種算法在新的障礙物環(huán)境中的避障效果對比。與圖14相比,環(huán)境中的障礙物位置發(fā)生了變化。對比兩圖可知,兩種算法都能實現(xiàn)避障,但在面對同一障礙物時,規(guī)劃的避障路徑卻不同。模糊控制器按照固定的規(guī)則,當(dāng)θ為M時,只能采取向左飛行規(guī)避障礙物的策略。而模糊神經(jīng)網(wǎng)絡(luò)控制器則能根據(jù)訓(xùn)練數(shù)據(jù),提取障礙物特征,根據(jù)不同的障礙物采取更優(yōu)的避障策略。 圖15 模糊控制、模糊神經(jīng)網(wǎng)絡(luò)仿真結(jié)果對比 圖16為在該環(huán)境中采用兩種避障方法的無人機(jī)飛行狀態(tài)對比圖。模糊控制規(guī)劃路徑長度為1 046.4 m,模糊神經(jīng)網(wǎng)絡(luò)規(guī)劃路徑長度為938.7 m。由圖可知,在118 s時無人機(jī)采取了不同的避障動作,路徑抵達(dá)目標(biāo)點的時間分別為244 s和271.75 s。模糊神經(jīng)網(wǎng)絡(luò)縮短了避障路徑,能夠更快到達(dá)目標(biāo)點。 圖16 飛行狀態(tài)對比 圖17為其中另一新環(huán)境中的避障效果對比,該環(huán)境中存在局部的“凹多邊形”障礙物區(qū)域。 圖17 隨機(jī)環(huán)境中模糊控制、模糊神經(jīng)網(wǎng)絡(luò)仿真結(jié)果對比 模糊神經(jīng)網(wǎng)絡(luò)經(jīng)過大量避障數(shù)據(jù)的訓(xùn)練,能夠提取障礙物分布的深度信息,其輸出是輸入的線性組合,并不依賴固定的規(guī)則。當(dāng)無人機(jī)進(jìn)入到“凹多邊形”區(qū)域內(nèi)時,能夠根據(jù)障礙物特征,做出“后退”的飛行動作,從“陷阱”中脫離。 最后,通過蒙特卡羅仿真實驗比較模糊控制與模糊神經(jīng)網(wǎng)絡(luò)算法的避障效果,驗證模糊神經(jīng)網(wǎng)絡(luò)具有更強(qiáng)的適應(yīng)性。首先以文中的7個障礙物模型為基礎(chǔ),隨機(jī)更改它們的位置,生成了500張障礙物地圖。然后無人機(jī)在兩種控制算法下分別進(jìn)行避障仿真,并統(tǒng)計無人機(jī)的避障次數(shù)、路徑長度與飛行時間。實驗結(jié)果如表3所示。 表3 避障仿真實驗結(jié)果 在500次的仿真實驗中,模糊神經(jīng)網(wǎng)絡(luò)算法的避障成功率更高,平均路徑更短,飛行時間更少,說明了該算法有更強(qiáng)的適應(yīng)性。 但是由于無人機(jī)在未知環(huán)境中規(guī)劃的是局部路徑,且模糊神經(jīng)網(wǎng)絡(luò)算法沒有考慮障礙物信息處理、環(huán)境信息融合等問題,因此依然存在部分“凹多邊形”區(qū)域影響無人機(jī)的避障效果。另外,由于全局環(huán)境信息未知,模糊神經(jīng)網(wǎng)絡(luò)只能得到可行解,并非最優(yōu)解。針對未知環(huán)境中如何得到最優(yōu)路徑問題,還需更深入的研究與探討。 本文采用等效夾角優(yōu)化了模糊控制器的輸入變量,將模糊控制應(yīng)用于無人機(jī)未知環(huán)境中的避障,生成了避障路徑。然后提取路徑數(shù)據(jù),設(shè)計并訓(xùn)練模糊神經(jīng)網(wǎng)絡(luò),并進(jìn)行了蒙特卡羅仿真實驗,比較了兩種算法的避障效果。結(jié)果表明模糊神經(jīng)網(wǎng)絡(luò)算法的適應(yīng)性更強(qiáng),能夠優(yōu)化局部避障路徑,規(guī)避分布較為復(fù)雜的障礙物,在未知環(huán)境中的避障成功率更高。2 基于模糊控制的避障算法
2.1 模糊控制器輸入輸出變量定義
2.2 模糊控制器隸屬度函數(shù)定義
3 模糊神經(jīng)網(wǎng)絡(luò)避障算法
3.1 模糊神經(jīng)網(wǎng)絡(luò)
3.2 模糊神經(jīng)網(wǎng)絡(luò)的結(jié)構(gòu)和參數(shù)訓(xùn)練流程
4 仿真結(jié)果
4.1 仿真條件
4.2 模糊控制避障仿真結(jié)果
4.3 模糊控制與模糊神經(jīng)網(wǎng)絡(luò)避障仿真對比
5 結(jié)語