基于超聲波測距和多普勒測速的室內無人機跟蹤定位算法

黃 婷，徐 明

(江西財經職業學院南昌校區信息中心，南昌 331700)

0 引言

隨著電子通信技術的發展，無人機(UAV)已在環境監測，農業植保，影像航拍等多個領域中廣泛使用[1]。位于地面上的控制基站通過向無人機的航行控制器發送指令，使無人機依據用戶的指令飛行。因此，控制基站需實時對無人機進行定位和跟蹤。

目前，借助于全球定位系統(GPS)和無人機的慣性測量裝置(IMU), 控制基站能夠方便對室外環境的無人機進行定位跟蹤[2]。然而，由于GPS在室內環境的定位精度低，對室內環境的無人機進行定位跟蹤存在挑戰。

針對室內環境的無人機定位問題，研究人員提出多種定位算法。其中基于視覺定位算法受到廣泛關注[3]，但由于無人機在飛行中的震動，基于視覺定位算法的精度并不高。研究人員提出了其他的定位算法，如基于射頻定位[4]、基于多普勒頻移跟蹤[5]以及基于蜂窩網絡跟蹤等[6]。但是在室內環境，蜂窩網絡信號不穩，容量受到環境影響。

超聲波測距和多普勒測速的跟蹤定位算法(TLUD)先估計超聲波信號的到達時間差ToA[7]，利用跳頻擴頻(FHSS)技術消除室內多徑衰減的影響，提高基于ToA的測距精度，利用卡爾曼濾波估計實現實時測距，并利用多普勒頻移估計無人機的移動速度。仿真結果表明，提出的TLUD算法有效提高了室內定位精度。

1 TLUD定位跟蹤算法

系統模型如圖1所示，無人機在室內飛行，通過WI-FI與終端用戶進行數據通信。無人機內安裝了超聲波發射器，周期地傳輸超聲波信號。接收終端收集數據，并執行定位算法，最終對無人機進行定位跟蹤。

圖1 系統模型

1.1 基于ToA的測距

無人機發射信號，經二相相移鍵控(BPSK)調制[8]，再通過擴頻傳輸至用戶。BPSK調制有兩個變化的狀態，一個狀態表示一位比特信息。如用相位+90°表示1，相位-90°表示0。如圖2(a)所示。令S(t)表示無人機發射的BPSK調制信號，其表達式為：

S(t)=b·T(t)·sin(2πfmt+φ)

(1)

式中：b為數據符號；T(t)為矩形脈沖函數；TB為矩形脈沖的寬度，當0≤t≤TB，T(t)=1，否則為零，如圖2(b)所示；fm為信號跳頻點的頻率集；φ為初始相位。

圖2 BPSK和矩形脈沖信號

信號S(t)經多徑信道傳輸至接收端,接收端所接收的信號r為：

r(t-τ)=d·T(t-τ)·sin(2πfm(t-τ)+φ)+M(t)+N(t)

(2)

式中：τ為傳輸時延；M(t)為多徑衰落的影響;N(t)為高斯噪聲。M(t)可表示為：

(3)

式中：αi為第i條路徑的信號衰減；τi為第i條路徑信號的時延。多徑衰弱在室內環境無法避免。為此，利用FHSS技術[9]消除多徑衰弱，進而提高對時延τ的估計。因此，式(2)可轉換成：

r(t-τ)=b·T(t-τ)·sin(2πfm(t-τ)+φ)+N(t)

(4)

接下來，利用廣義互相關估計時延τ，令R1,2(τ)表示信號S(t)與r(t-τ)的互相關值，其表達式為：

R1,2(τ)=E{S(t)·r(t-τ)}

(5)

(6)

1.2 基于多普勒頻移測速

當信號接收端與發送端間相對位置發生移動時，信號頻率就發生偏移。令Fs表示頻移的頻率，其表達式為：

Fs=(υ/c)F0

(7)

式中：F0為原始信號頻率；υ為相對于發送端的接收端的相對速度；c為聲波的傳輸速度。

1.3 基于Kalman濾波的測距修正

充分利用所測的距離和1.2節所測的速度信息，Kalman濾波實時地估計無人機與控制基站(接收端)間的距離。 令Dk表示在第k個時間窗口內，無人機與控制基站的實際距離：

(8)

(9)

(10)

式中：σk，ζk分別為nk，ωk的標準方差。

1.4 基于泰勒級數展開的最小二乘法的無人機位置估計

令(x,y,z)表示無人機位置。估計三維坐標的位置，至少需要建立3個方程：

(11)

式中：i=1,2,3。di表示第i個超聲波接收端離UAV的距離。該距離是依據式(10)所估計距離。

1.4.1 基于最小二乘法的位置估計

考慮到測距誤差，di3總是與實際值存在誤差。若只依據式(11)所構建的3個方程求解無人機位置，位置誤差會較大。為此，增加方程數量，并利用基于泰勒級數展開的最小二乘法求解，進而提高位置估計的精度。

4S的營銷戰略告訴我們從消費者需求出發，打破傳統的追求市場占有率營銷模式，建立起一種全新的“消費者占有”的營銷導向。通過滿意(satisfaction)、服務(SERVICE)、速度(speed)以及誠意(sincerity)四個方面來不斷滿足消費者的消費需求。要求對產品、服務、品牌不斷進行綜合性消費者滿意指數和消費者滿意級度的測評與改進，以服務品質最優化，使消費者滿意度最大化，進而達到消費者忠誠的“指名度”，同時強化了企業的抵御市場風險經營管理創新和持續穩定增效的“三大能力”。

建立4個方程,即在空間內部署4個超聲波接收端，并由這4個接收端離無人機的距離，估計無人機的位置。因此，結合式(11)可得:

(12)

X=(ATA)-1ATB

(13)

1.4.2 基于泰勒級數展開

對式(12)進行線性化處理丟失了一部分測距信息，這可能降低了估計精度。為此，再利用泰勒級數展開，進一步估計無人機的位置。

考慮到式(12)為2階方程組，先對式(12)進行開方運算，再改寫成關于x,y,z的函數得：

(14)

(15)

經簡化整理可得：

MX=L+E

(16)

X=(MTQ-1M)-1MTQ-1L

(17)

2 性能分析

2.1 仿真環境

利用MATLAB軟件建立仿真，分析TLUD算法的性能。在5 m×5 m×3 m的房間內部署一個無人機和4個超聲波接收端(4個智能手機)。4個智能手機的位置分別為：(2.5,0,1.5)，(5,2.5,2.5)，(2.5,5,2)和(0,5,3),單位為m。無人機在室內隨機飛行。

在無人機內部安裝一個超聲波發射器，并由該發射器傳輸FHSS信號。頻率范圍為25～55 kHz。抽樣頻率fs=340 kHz。在25～55 kHz頻率區間內選取6個子頻率(跳頻)：27.5 kHz,32.5 kHz,37.5 kHz,42.5 kHz,47.5 kHz和52.5 kHz。每個子頻的的帶寬為5 kHz。

2.2 TLUD算法的定位跟蹤的性能

首先，分析平均定位誤差隨未知節點數變化情況，其中未知節點數從10至150變化，如圖3所示。

圖3 TLUD算法對無人機的定位跟蹤性能

從圖3可知，TLUD算法能夠有效地跟蹤目標(無人機)。圖中的紅色虛線表示無人機的實際飛行軌跡；綠色實線表示估計的飛行軌跡。這兩條線基本重合。這說明TLUD算法能夠有效定位無人機，并實時跟蹤無人機。

圖4繪制了TLUD算法在x軸、y軸和z軸3個維度的定位誤差。從圖4可知，定位誤差隨接收信號的信噪比的增加而下降，這符合預期。此外，由圖3可得，在z軸的定位誤差大于在x軸、y軸的定位誤差。原因在于幾何精度因子(GODP)的衰減，擴大了z軸的定位誤差。

圖4 x軸、y軸和z軸的定位誤差

2.3 平均定位誤差的對比分析

選擇只通過ToA測距，不考慮速度估計和Kalman濾波的定位算法作為參照，記為TLUD-uK。實驗中，對無人機的7條飛行軌跡進行定位，定位結果如圖5所示。

圖5 平均定位誤差

從圖5 (a)可知，TLUD算法在x-y軸平面的定位誤差為0.22 cm，遠低于TLUD-uK。這說明利用多普勒測速，并通過Kalman濾波可以提高測距精度，進而提高定位精度。

圖5(b)繪制了TLUD算法在x-y-z軸平面的定位精度，平均定位精度約0.55 cm。對比圖5(a)和圖5(b)不難發現，在x-y-z軸平面的定位誤差大于在x-y軸平面的定位誤差。原因在于z軸的定位誤差遠大于在x軸和y軸的定位誤差。

3 總結

針對室內無人機的跟蹤定位進行研究，提出跟蹤定位算法TLUD。TLUD算法先利用超聲波測距，并利用多普勒效應估計無人機的速度，再通過卡曼濾波算法融合測距和測速數據，最終利用最小二乘法跟蹤無人機位置。仿真數據表明，提出的TLUD算法室內平均定位誤差約為0.55 cm，能夠有效地估計無人機位置。