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

佘凱， 趙振華， 田江波

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

0 引言

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

1 無人機避障路線智能生成方法設計

1.1 障礙物過濾和檢測

1.1.1 障礙物數據的過濾及聚類

由于飛行環境復雜，需要對絕對坐標實施去噪處理，通過激光點聚類的方法將無人機發出的所有激光點按照一定順序實施聚類處理，聚類的判斷規則為

(1)

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

(2)

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

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

(3)

1.1.2 潛在障礙物匹配

經過對障礙物的特征參數提取等處理，通過關聯性評估分析分割后的激光點數據[5]，障礙物之間的關聯評估為

(4)

其中，n為障礙物子集合激光點數目的差值，F為障礙物子集合質心的間距，G為2個障礙物不重合面積占總面積的比值。根據關聯評估函數的大小可判斷2個障礙物重復的可能性，函數越大，可能性越大。判斷出3種障礙物子集合。

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

在上述基礎上，建立目標障礙物測量方程[6]，將障礙物信息中存在的高斯白噪聲進行處理，處理后的狀態向量Xk為

Xk=(xk,yk,zk)

(5)

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

Zk=Vk+HXk

(6)

其中，Zk為k時刻的系統觀測值，Vk為正態分布的測量噪聲，H為激光器測量矩陣。

預測目前系統的狀態值和誤差協方差，如式(7)：

(7)

(8)

式中，K為卡爾曼增益，Xk為全新的協方差矩陣，Pk為系統狀態。

構建完障礙物檢測地圖后得出障礙物的絕對坐標為

(9)

其中，ε為2個坐標系之間軸的旋轉角度，ΔX0、ΔY0、ΔZ0為2個坐標系原點坐標之間的差值。

1.3 智能生成無人機避障路線

1.3.1 求解無人機的飛行方向

(1) 將無人機可掃描的區域劃分成n個區域，并從小到大逆時針的方向對n個區域編號，令第i個區域為Zi，i=1,2,…,n。

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

(10)

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

經實驗發現，在接近目標障礙物的可飛行范圍內，閾值越大越好，在此條件的基礎上求解最優飛行方向。

1.3.2 控制角速度

求最優飛行方向的過程如下。

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

(11)

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

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

(3) 求出第i個智能無人機的飛行方向為{(θi,Ri,Li)||i=1,…,N}，θi是所有可飛行方向中第i個飛行方向與無人機坐標系x軸的夾角，Ri是第i個飛行方向的閾值，Li是第i個飛行方向所對應扇區的弦長。

選取最大閾值Rs為最優飛行方向，其中|θi-θtarget|≤δ，θtarget為目標障礙物對應智能無人機的方向，δ為事先確定的常數。

(4) 角速度如下：

ω=kωθe

(12)

1.3.3 控制線速度

|θe|較大時，需將智能無人機的飛行線速度進行減速，則其定義為

(13)

式中，vmax為無人機最大飛行速度，vmin為無人機最小飛行速度，Rmax為最大閾值，Rs為求解出的最大閾值，xm為|θe|在最大值且Rs=R0時xv的取值，w為權重因子。經過對智能無人機的角速度和線速度的控制實現自動生成避障路線。

2 實驗結果與分析

為了驗證基于卡爾曼濾波算法的無人機避障路線智能生成方法的整體有效性，設計實驗，實驗場地為某無人機試飛區。實驗前的準備工作如下：開啟計算機控制系統，確保實驗所需的傳感器能正常采集數據；初始化調試系統；為使生成路線結果更為精確，將180個障礙物標定好，保障所得數據為有效數據。

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

圖1 3種方法避障結果

圖1中矩形區域為障礙物設置區域。分析圖1可知，其他2種方法都不能自動生成最優避開障礙物的路線，而本文方法通過卡爾曼濾波算法提取障礙物位置信息，提前檢測到了障礙物，提高了無人機避障的效果。

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

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

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

3 總結

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