單基站基于波達角度的剛體位置姿態最大似然估計

姚曉峰,吳圣蘭,周彪,龐敏

(1. 江蘇省物聯網應用技術重點建設實驗室(無錫太湖學院),無錫 214122;2. 江南大學 物聯網工程學院,無錫 214122)

0 引 言

剛體目標是指一種在運動或受力時仍然保持其外部拓撲結構的物體,包括航天器、虛擬現實應用中使用的頭盔或智能機器人．剛體位置和方向估計方案用來估計剛體相對于參考狀態的中心坐標和旋轉歐拉角[1-2]．與傳統的獲取目標的點位置不同,剛體定位(RBL)旨在同時獲取剛體的位置和姿態信息．

剛體的高精度位置和姿態信息在航天、軍事和智能工業領域有著廣泛的應用前景[3-6]．在虛擬現實應用中,虛擬現實系統必須根據用戶頭部的準確位置和姿態提供相應的虛擬圖像．在室內精密導航系統[7]中,車輛的位置和方向是智能停車系統所需要的基本信息．航天體的對接也需要精確的航天器姿態和相對位置信息．

目前,基于導航衛星的定位與姿態測量系統是較為主流的室外大型RBL測姿方案[8-9]．衛星載波相位所涉及的整周模糊度分辨問題影響了此方案的穩定性．另外,其應用在非衛星視距場景下的限制也較為明顯．利用視覺圖像處理的位置和方向估計方案是RBL測姿的一種常用方案[10]．但是該方案的精度過分依賴于特征點的低誤差坐標觀測值,并且視覺圖像處理的技術特點也導致其對光照條件的魯棒性差,同時計算量較大．

基于小規模無線傳感器網絡的RBL與姿態測量方案具有實現成本低、對室內外環境和光照條件適應性強等優點[11-12]．基于小規模傳感器網絡估計方案的實現可以總結如下:首先,將無線傳感節點附著在剛體表面,形成一個拓撲結構已知的小規模傳感網絡;然后,基站對網絡中的每個無線節點的信號到達特征進行觀測;最后,通過優化算法融合上述信息得到剛體的三維坐標和方位信息．然而,由于含有姿態信息的三維特殊正交群必須滿足酉矩陣的單位正交條件,如何優化該模型成為一個更具挑戰性的問題．目前,基于無線傳感器網絡的定位測姿估計方案的研究主要是基于到達時間(TOA)的觀測．然而為了保證系統的精度,基于TOA的定位測姿方案需要3～4個基站,增加了系統實現成本的同時也限制了其應用場景．并且由于基站與傳感器節點之間存在嚴格的時鐘同步問題,基于TOA的定位與姿態估計方案的實用性較差．

基于觀測到達方向(DOA)的定位測姿估計方案可以完全避免時鐘同步問題,提高系統的可行性．除此之外,在基站獲得的二維DOA信息比TOA信息具有更高的維數,這使得單基站定位方向成為可能,成熟的DOA估計方法也為基于DOA的定位測姿方案奠定了實施基礎[13-15]．

1 單基站基于波達角度的剛體定位

框架

本節將介紹基于DOA測量的單基站RBL框架．為了確定三維空間中剛體的狀態(包括位置和姿態),在其表面安裝無線節點作為特征點,形成一個小規模的無線傳感器網絡．此傳感網絡的拓撲信息事先是完全知道的,并且在移動和旋轉時保持不變．

方便起見,將單基站位置設置為原點．單基站上配備了一個二維天線陣列進行測量來自傳感網中每個節點的信號到達的方向．為了描述無線傳感器網絡當前的狀態,需要預先定義一個已知的參考幀(幀由各個節點的三維位置構成的)．當前幀,即當前剛體的狀態,假設是通過參考幀旋轉和平移得到(如圖1所示),在數學上表示為

E=QC+t?1(1×K),

(1)

式中，C=[c1,…,cK]∈R(3×K)和E=[e1,…,eK]∈R(3×K)分別表示參考幀和當前幀．在C和E中,第k個節點的三維坐標位置分別表示為ck=[cx,k,cy,k,cz,k]T和ek=[ex,k,ey,k,ez,k]T．在式(1)中,t=[x,y,z]T是未知的平移向量;Q是未知的旋轉矩陣,屬于三維特殊正交矩陣

(2)

對于廣義RBL問題,其任務是估計旋轉矩陣Q和平移向量t．從而通過求出未知的當前幀和已知的參考幀的關系來描述剛體當前的位置和姿態．

傳感器網絡的拓撲信息是精確已知的,且在旋轉平移中不變,那么可以用節點對的歐氏距離來描述此拓撲信息

圖1 單基站剛體定位測姿場景

d=[d1,2,…,di,j,…,dK-1,K]T,i,j=1,…,

K,i

(3)

式中：di,j=ci-cj=ei-ej是第i和第j節點之間的歐式距離；d是一個×1向量．

另一個信息是傳感器節點到基站原點的波達角度．即當前幀中第k個節點ek信號傳播至原點的方向與x軸和z軸之間的夾角為αk和βk．為了方便起見,將αk和βk映射為投影角θ=[θ1,…,θK]T,即信號傳播至原點的方向和平面xoy和zoy的夾角,其映射方法表示如下:

(4)

其中

θ=[θ1,…,θK]T,

(5)

2 基于DOA的RBL框架的最大似

然估計

為了估計表示剛性目標的姿態和位置的旋轉矩陣Q和平移向量t,本節介紹了兩種最大似然估計算子從不同的角度計算旋轉矩陣和平移向量．第一種算法是先求出各個節點的三維坐標E,然后用單位四元數法求出平移向量和旋轉矩陣．另一種方法是將平移向量和旋轉矩陣作為未知量直接求出．

2.1 拓撲匹配最大似然估計

第一種基于最大似然估計的RBL框架建立分為兩個階段:建立最大似然估計來計算當前幀中節點的三維坐標,然后采用點匹配算法找到Q和t．

首先使用投影角作為量測,是為了簡化當前幀中的節點坐標ek和θk之間的關系:

ek=[tanθk,x,1,tanθk,z]T*ey,k.

(6)

如此一來,當前幀的3×K個三維坐標未知數減少為1×K個未知數,即

φ1=ey=[ey,1,…，ey,k,…,ey,K]T．

然后利用無線節點的確切已知拓撲信息d建立最大似然函數．d和待估計值φ1的關系為

d(φ1)=ei-ej,i,j=1,…,K,i

(7)

(8)

(9)

利用高斯-牛頓法求出使得式(8)最小的ey的解,將ey代入(6)即可得到無線節點在當前幀中的所有節點的三維坐標．最后,利用單位四元數法或奇異值分解法,即可得到剛體的平移向量和旋轉矩陣．

實質上,拓撲匹配最大似然估計是通過獲取K傳感器的ey坐標來實現RBL的,其匹配信息是K(K-1)/2個距離．而待估計的參數個數為K個ey,k,故K>3為估計成功的必要條件．

2.2 測量匹配最大似然估計

第二種最大似然估計是直接計算旋轉角度和平移向量,要匹配的信息是DOA信息,即稱測量匹配最大似然估計．現場實施過程中可以精確測得節點對距離信息,然后利用多維尺度算法[16](MDS)對節點距離信息進行計算處理得到參考幀(在本文的仿真工作中,參考幀是預先確定的,而不是通過MDS算法來融合節點對距離得到)．

旋轉矩陣可以由三個歐拉角Υ=[φp,φy,φr]T決定,φp,φy,φr分別為俯仰角、偏航角和翻滾角．三個角度決定了旋轉矩陣為:

Q(Υ)= [r1,r2,r3]T=

(10)

式中,s*、c*分別代表sinφ*以及cosφ*．因此,包括平移向量t=(x,y,z)T在內,有6個未知數φ2=[ΥT,tT]T．由于距離已被融合以獲得參考幀,唯一可用的條件是DOA測量值．K個節點產生2×K個DOA．根據式(1)和式(6),DOA測量值與6個未知量之間的關系可以表示為θ=θ(φ2),具體為

(11)

由于波達方向是獨立測量的,并且與零均值高斯白噪聲混合(見式(5)),θ的最大似然函數可以表示為

(12)

(13)

得到最優解φ2后,把γ代入方程(10)得到Q．這就完成了t和Q估計．

需要注意的是,通過分析使用條件和要估計的參數的數量,上述兩個估計需要剛體表面至少綁定三個或以上的無線節點,方可來完成RBL估計任務．

3 最大似然估計高斯-牛頓算法

為了尋找使代價函數(9)和(13)最小化的最優解,可以采用多種優化方法．在這里高斯-牛頓算法被用來實現這個任務．如圖2所示.

圖2 高斯-牛頓迭代法中的初值重置

高斯-牛頓迭代法的基本思想是用泰勒級數展開法逼近非線性回歸模型,然后經過多次迭代,多次修改回歸系數,使回歸系數不斷逼近非線性回歸模型的最佳回歸系數,最后使原模型的殘差平方和最小．上述兩種最大似然估計問題可以表示為

(14)

(15)

這是一個非線性最小二乘問題．

為了將此最小二乘問題線性化,在初始值φ0處對式(7)和式(11)進行泰勒級數展開,并忽略階數大于2的項,即可得到

(16)

在第t次迭代時,當前信息向量φ(t)和最優解φ之間的差距可以從等式(17)中獲得

Δ=φ-φ(m)=(G*TG*)-1G*T(Θo-Θ(φ(t)))

(17)

在下一次迭代中,可以通過替換φ(t+1)=φ(t)+Δ來更新迭代值．上述迭代直到優化收斂ΔTΔ降低到小閾值以下或達到最大迭代次數為止．

從經驗上講,傳統的牛頓法在初值φ(0)不接近基準值時容易達到局部最優,因為所述RBL模型為非凸模型．因此可以通過增加收斂性判據對經典牛頓法進行了修正．在停止迭代后,將計算最終得到的估計值φ(tmax)與通過‖Θo-Θ(φ(tmax))‖實際測量的信息向量之間的相似度ρ;同時對估計值進行判斷是否符合先驗信息范圍．如果ρ大于預先設定的閾值或者不符合先驗信息范圍,則丟棄收斂結果,根據先驗知識隨機設置φ(0)重置φ,重新開始牛頓迭代．

待估計的參數,包括拓撲匹配方案的ey、DOA匹配方案的t和Q．作為空間相關參數的先驗知識,它們的范圍可以根據監測區域的房間大小粗略確定或者通過接受信號強度進行測量．此外,DOA匹配方案的歐拉角γ的范圍明顯在0到2π(rad)之間．若ρ一直達不到閾值,則重置初始值500次后退出整個參數估計過程．

4 性能評估

本文對所提出的RBL方案在一個2.4 GHz四核處理器的PC機上用MATLAB進行了仿真實驗,考慮了不同的情況下(不同的剛體尺寸D和不同的DOA測量噪聲σ2),從收斂成功率和計算成本以及均方根誤差(RMSE)三個方面評估了這兩種最大似然估計子．仿真參數定為K=4,那么給出的初始幀如下:

(18)

作為剛性目標的先驗狀態信息,相對于初始幀,當前幀位于x∈[0,9]∩y∈[0,9]∩z∈[0,5] m的三維空間,單BS置于原點．同時,三個旋轉角度從[0，2π]rad進行隨機設置．

首先,比較了三種估計在6種不同噪聲水平下當D=1 m和3 m的估計算法的魯棒性．本文所提出的最大似然估計子對每一種場景分別進行1 000次獨立的蒙特卡羅運行,在每一次運行中隨機選擇t和Q的基準值,并根據先驗知識隨機選擇改進的高斯-牛頓法的初始解．因此,基準值和初始解的選擇都是根據已知的先驗信息范圍內得到的,然后估計結果與基準值進行比對,以判斷收斂的成功與否．

兩種估計子的收斂成功率如圖3所示．正如所期望,在DOA測量噪聲水平和系統性能成反比的同時,較大的剛體尺寸下,兩種估計方案也能獲得較高的收斂成功率．這是因為較大的剛體尺寸也就意味著更大的DOA測量差值,也就提高了估計算法的魯棒性．

橫向來看,基于DOA測量匹配最大似然估計方案的魯棒性優于基于拓撲信息匹配方案,前者的收斂成功率始終保持在95%以上,即使在最惡劣的情況下(當D=1 m時最高噪聲水平σ=1 deg)．這是一個較為出色的性能,因為牛頓迭代法對于初值是相當敏感的,在改進的方案中,通過初值重置,基于DOA測量匹配最大似然估計也能具有良好的性能,而拓撲匹配方案的收斂成功率仍然較低．直觀地說,從高斯-牛頓法的角度來說,匹配條件越多(在K=4個節點的情況下,測量匹配方案的8個條件,包括8個DOA測量值;拓撲匹配方案的6個條件,包括6個節點對距離),測量匹配最大似然估計方案對測量噪聲的魯棒性越強,因此DOA測量匹配最大似然估計方案性能高于拓撲匹配．圖3也給出了RBL方法的計算成本．在一般意義上,待估計參數的維數越高,收斂速度越慢．然而可以看出基于測量匹配最大似然估計方案計算速度卻更加有效．這是因為其較高的收斂成功率減少了牛頓算法中初始值重置的次數．相反,拓撲匹配方案顯然需要最大的計算資源．還應注意到的是,DOA測量匹配最大似然估計方案在低級測量噪聲下是有效的,而隨著噪聲的增加,其效率會降低．

(a)拓樸信息匹配最大似然估計方案

(b)DOA測量匹配最大似然估計方案圖3 兩種最大似然RBL估計算法的計算成本和收斂成功率分析

圖4和圖5展示了所有估計方案在D=1和D=3時的RBL精確度．本文選擇拓撲匹配方案中的收斂成功結果計算RMSE,但與克拉美羅界相比仍有明顯的差距,特別是對于小型剛體．這與上面的分析保持一致,即使用的匹配條件越多,性能越好．DOA匹配方案利用了更多的匹配條件,在較高的DOA噪聲水平下可以達到克拉美羅界,但與克拉美羅界的性能差距越來越大．此現象可能是由于旋轉角度與旋轉矩陣之間轉換過程中的高度非線性造成的．

DOA噪聲標準差/deg(a)平移矢量t估計

DOA噪聲標準差/deg(b)放置矩陣Q估計圖4 當D=1 m時RBL估計精度比較

DOA噪聲標準差/deg(a)平移矢量t估計

DOA噪聲標準差/deg(b)旋轉矩陣Q估計圖5 當D=3 m時RBL估計精度比較

5 結論和展望

本文提出了基于DOA的單基站RBL框架的兩種最大似然估計方法,一是通過匹配拓撲信息來估計當前幀中節點的坐標,然后估計位置姿態,稱為拓撲匹配最大似然估計方案;二是利用DOA測量直接求出最優的平移向量和旋轉角度,稱為DOA測量匹配最大似然估計方案．采用改進的牛頓-高斯算法對兩個MLE函數進行優化,根據先驗信息對初值進行有規律的重置,以降低收斂失敗率．最后,從收斂成功率、計算代價和精度方面對兩種估計方案在不同情況下進行評估．結果表明,在不同情況下,第二種方案可以在保持95%以上的收斂成功率的同時,在高噪聲情況下其RMSE近似于克拉美羅界．

這項工作只考慮到拓撲信息是精確已知的,當剛性目標來自第三方時,可能無法滿足這一要求．因此,在未來的研究中,只需大致了解RBL的拓撲結構,便可以研究噪聲拓撲信息對RBL性能的影響．在完成RBL任務的同時,只利用DOA測量和粗糙拓撲信息對噪聲拓撲信息進行校正,將是一個有趣的研究課題．