基于卡爾曼濾波算法的無人機(jī)避障路線智能生成方法

佘凱， 趙振華， 田江波

(國網(wǎng)河北省電力有限公司保定供電分公司， 河北， 保定 071000)

0 引言

隨著無人機(jī)技術(shù)的成熟發(fā)展，無人機(jī)在軍事和民用領(lǐng)域發(fā)揮著非常重要的作用。無人機(jī)能長時(shí)間進(jìn)出各種危險(xiǎn)空域，具有體積小、維護(hù)簡單、維護(hù)方便、靈活性好等優(yōu)點(diǎn)，具有良好的發(fā)展前景[1]。針對(duì)解決無人機(jī)避障問題有諸多研究，其中：張啟錢等[2]提出了基于IOCAD的無人機(jī)避障路線自動(dòng)生成方法，建立障礙物避碰檢測模型，采用射線法篩選出環(huán)境中可飛路徑點(diǎn)，求解出遇到不規(guī)則障礙物時(shí)無人機(jī)的飛行路徑，實(shí)現(xiàn)無人機(jī)避障路線自動(dòng)生成，但該方法沒有提取障礙物位置信息，避障線路可能存在誤差；代進(jìn)進(jìn)等[3]提出了基于模型預(yù)測控制的無人機(jī)避障路線自動(dòng)生成方法，通過限時(shí)域優(yōu)化的算法實(shí)現(xiàn)無人機(jī)避障路線自動(dòng)生成，但該方法沒有在獲取障礙物位置信息時(shí)對(duì)信息進(jìn)行濾波處理，導(dǎo)致障礙物的檢測率低；唐立等[4]提出了基于改進(jìn)蟻群算法的無人機(jī)避障路線自動(dòng)生成方法，考慮了無人機(jī)性能參數(shù)對(duì)拐角進(jìn)行平滑處理，獲得可用于實(shí)際飛行的最優(yōu)安全路徑，實(shí)現(xiàn)無人機(jī)避障路線自動(dòng)生成，但該方法無法匹配出潛在障礙物的位置信息，造成定位障礙物的誤差率高。為了解決上述研究中存在的問題，本文提出基于卡爾曼濾波算法的無人機(jī)避障路線智能生成方法，解決無人機(jī)路線誤判的問題，采用激光器掃描障礙物信息，提取障礙物坐標(biāo)并生成無人機(jī)自動(dòng)避障路線，實(shí)現(xiàn)無人機(jī)高效智能避障。

1 無人機(jī)避障路線智能生成方法設(shè)計(jì)

1.1 障礙物過濾和檢測

1.1.1 障礙物數(shù)據(jù)的過濾及聚類

由于飛行環(huán)境復(fù)雜，需要對(duì)絕對(duì)坐標(biāo)實(shí)施去噪處理，通過激光點(diǎn)聚類的方法將無人機(jī)發(fā)出的所有激光點(diǎn)按照一定順序?qū)嵤┚垲愄幚?，聚類的判斷?guī)則為

(1)

其中，di,i+1為激光點(diǎn)集中每兩點(diǎn)的間距，r為聚類搜索半徑，nmin為激光點(diǎn)聚類分割后最少部分的聚類，N為聚類所需的最少數(shù)目點(diǎn)。分析每個(gè)障礙物中所有特征點(diǎn)確定障礙物運(yùn)動(dòng)狀態(tài)，其描述如下：

(2)

第二步，障礙物子集合內(nèi)的橫坐標(biāo)和縱坐標(biāo)的取值范圍分別為[xkmin,xkmax]、[ykmin,ykmax]，由此可求解出障礙物的面積Sk為

Sk=(xkmax-xkmin)(ykmax-ykmin)

(3)

1.1.2 潛在障礙物匹配

經(jīng)過對(duì)障礙物的特征參數(shù)提取等處理，通過關(guān)聯(lián)性評(píng)估分析分割后的激光點(diǎn)數(shù)據(jù)[5]，障礙物之間的關(guān)聯(lián)評(píng)估為

(4)

其中，n為障礙物子集合激光點(diǎn)數(shù)目的差值，F(xiàn)為障礙物子集合質(zhì)心的間距，G為2個(gè)障礙物不重合面積占總面積的比值。根據(jù)關(guān)聯(lián)評(píng)估函數(shù)的大小可判斷2個(gè)障礙物重復(fù)的可能性，函數(shù)越大，可能性越大。判斷出3種障礙物子集合。

1.2 基于卡爾曼濾波算法提取障礙物位置信息

在上述基礎(chǔ)上，建立目標(biāo)障礙物測量方程[6]，將障礙物信息中存在的高斯白噪聲進(jìn)行處理，處理后的狀態(tài)向量Xk為

Xk=(xk,yk,zk)

(5)

其中，xk為無人機(jī)系統(tǒng)在正北方向上的位移，yk為無人機(jī)系統(tǒng)在正東方向上的位移，zk為無人機(jī)系統(tǒng)在豎直方向上的位移。系統(tǒng)離散化后的觀測方程為

Zk=Vk+HXk

(6)

其中，Zk為k時(shí)刻的系統(tǒng)觀測值，Vk為正態(tài)分布的測量噪聲，H為激光器測量矩陣。

預(yù)測目前系統(tǒng)的狀態(tài)值和誤差協(xié)方差，如式(7)：

(7)

(8)

式中，K為卡爾曼增益，Xk為全新的協(xié)方差矩陣，Pk為系統(tǒng)狀態(tài)。

構(gòu)建完障礙物檢測地圖后得出障礙物的絕對(duì)坐標(biāo)為

(9)

其中，ε為2個(gè)坐標(biāo)系之間軸的旋轉(zhuǎn)角度，ΔX0、ΔY0、ΔZ0為2個(gè)坐標(biāo)系原點(diǎn)坐標(biāo)之間的差值。

1.3 智能生成無人機(jī)避障路線

1.3.1 求解無人機(jī)的飛行方向

(1) 將無人機(jī)可掃描的區(qū)域劃分成n個(gè)區(qū)域，并從小到大逆時(shí)針的方向?qū)個(gè)區(qū)域編號(hào)，令第i個(gè)區(qū)域?yàn)閆i，i=1,2,…,n。

(3) 將相鄰的2個(gè)候選區(qū)間合并在一起形成準(zhǔn)飛行扇區(qū)域，合并出Nc個(gè)準(zhǔn)飛行扇區(qū)域，每一個(gè)準(zhǔn)飛行扇區(qū)域都由四維的向量Sj表示：

(10)

(4) 求得準(zhǔn)飛行扇區(qū)域Sj，(j=1,2,…,Nc)。

經(jīng)實(shí)驗(yàn)發(fā)現(xiàn)，在接近目標(biāo)障礙物的可飛行范圍內(nèi)，閾值越大越好，在此條件的基礎(chǔ)上求解最優(yōu)飛行方向。

1.3.2 控制角速度

求最優(yōu)飛行方向的過程如下。

(1) 假設(shè)Rmax=(NR-1)ΔR，Ri={(R0+i×ΔR)|i=0,1,…,NR-1}，其中Rmax為最大閾值，ΔR為固定增量，NR為常數(shù)，R0為最小閾值，則：

(11)

式中，a為減加速度，λ為放大系數(shù)，DS為無人機(jī)從最大速度到最小速度所飛行的距離。

(2) 所有的Ri(i=0,1,…,NR-1)均可由上述算法求解無人機(jī)的可飛行方向。

(3) 求出第i個(gè)智能無人機(jī)的飛行方向?yàn)閧(θi,Ri,Li)||i=1,…,N}，θi是所有可飛行方向中第i個(gè)飛行方向與無人機(jī)坐標(biāo)系x軸的夾角，Ri是第i個(gè)飛行方向的閾值，Li是第i個(gè)飛行方向所對(duì)應(yīng)扇區(qū)的弦長。

選取最大閾值Rs為最優(yōu)飛行方向，其中|θi-θtarget|≤δ，θtarget為目標(biāo)障礙物對(duì)應(yīng)智能無人機(jī)的方向，δ為事先確定的常數(shù)。

(4) 角速度如下：

ω=kωθe

(12)

1.3.3 控制線速度

|θe|較大時(shí)，需將智能無人機(jī)的飛行線速度進(jìn)行減速，則其定義為

(13)

式中，vmax為無人機(jī)最大飛行速度，vmin為無人機(jī)最小飛行速度，Rmax為最大閾值，Rs為求解出的最大閾值，xm為|θe|在最大值且Rs=R0時(shí)xv的取值，w為權(quán)重因子。經(jīng)過對(duì)智能無人機(jī)的角速度和線速度的控制實(shí)現(xiàn)自動(dòng)生成避障路線。

2 實(shí)驗(yàn)結(jié)果與分析

為了驗(yàn)證基于卡爾曼濾波算法的無人機(jī)避障路線智能生成方法的整體有效性，設(shè)計(jì)實(shí)驗(yàn)，實(shí)驗(yàn)場地為某無人機(jī)試飛區(qū)。實(shí)驗(yàn)前的準(zhǔn)備工作如下：開啟計(jì)算機(jī)控制系統(tǒng)，確保實(shí)驗(yàn)所需的傳感器能正常采集數(shù)據(jù)；初始化調(diào)試系統(tǒng)；為使生成路線結(jié)果更為精確，將180個(gè)障礙物標(biāo)定好，保障所得數(shù)據(jù)為有效數(shù)據(jù)。

采用基于卡爾曼濾波算法的無人機(jī)避障路線智能生成方法(方法一)、文獻(xiàn)[2]基于IOCAD的無人機(jī)避障路線自動(dòng)生成方法(方法二)和文獻(xiàn)[3]基于模型預(yù)測控制的無人機(jī)避障路線自動(dòng)生成方法(方法三)進(jìn)行測試。測試結(jié)果如圖1所示。

圖1 3種方法避障結(jié)果

圖1中矩形區(qū)域?yàn)檎系K物設(shè)置區(qū)域。分析圖1可知，其他2種方法都不能自動(dòng)生成最優(yōu)避開障礙物的路線，而本文方法通過卡爾曼濾波算法提取障礙物位置信息，提前檢測到了障礙物，提高了無人機(jī)避障的效果。

比較3種方法對(duì)障礙物的檢測率，如圖2所示。在多次迭代中比較3種方法對(duì)障礙物定位的誤差率，證明無人機(jī)避障路線的最優(yōu)生成辦法，如圖3所示。

圖2 3種方法對(duì)不同樣本中障礙物的檢測率

由圖2可知：本文方法對(duì)障礙物的檢測率均在87%以上；其他2種方法中噪聲過多，且沒有對(duì)數(shù)據(jù)進(jìn)行處理，導(dǎo)致影響檢測的因素過多，降低檢測率，進(jìn)而造成其他2種方法的檢測率不穩(wěn)定。由圖3可知，本文方法的誤差率始終控制在15%以下，均少于其他2種方法的誤差率，原因是本文方法利用卡爾曼濾波算法對(duì)采集的障礙物數(shù)據(jù)進(jìn)行濾波處理，獲取了障礙物的位置信息，提高了障礙物的檢測率和定位障礙物的準(zhǔn)確率。

3 總結(jié)

為了解決無人機(jī)避障路線生成中存在避障效果差、障礙物的檢測率低和定位障礙物的誤差率高的問題，提出基于卡爾曼濾波算法的無人機(jī)避障路線智能生成方法，通過獲取障礙物的位置信息，并根據(jù)障礙物的位置信息生成最優(yōu)避障路線，實(shí)現(xiàn)無人機(jī)精準(zhǔn)避障。今后研究中將考慮不同天氣和地區(qū)對(duì)無人機(jī)避障線路生成的影響，以及避障線路生成時(shí)間，以進(jìn)一步優(yōu)化設(shè)計(jì)所提方法，擴(kuò)大應(yīng)用范圍。