單基站基于波達(dá)角度的剛體位置姿態(tài)最大似然估計

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

(1. 江蘇省物聯(lián)網(wǎng)應(yīng)用技術(shù)重點建設(shè)實驗室(無錫太湖學(xué)院),無錫 214122;2. 江南大學(xué) 物聯(lián)網(wǎng)工程學(xué)院,無錫 214122)

0 引 言

剛體目標(biāo)是指一種在運動或受力時仍然保持其外部拓?fù)浣Y(jié)構(gòu)的物體,包括航天器、虛擬現(xiàn)實應(yīng)用中使用的頭盔或智能機器人．剛體位置和方向估計方案用來估計剛體相對于參考狀態(tài)的中心坐標(biāo)和旋轉(zhuǎn)歐拉角[1-2]．與傳統(tǒng)的獲取目標(biāo)的點位置不同,剛體定位(RBL)旨在同時獲取剛體的位置和姿態(tài)信息．

剛體的高精度位置和姿態(tài)信息在航天、軍事和智能工業(yè)領(lǐng)域有著廣泛的應(yīng)用前景[3-6]．在虛擬現(xiàn)實應(yīng)用中,虛擬現(xiàn)實系統(tǒng)必須根據(jù)用戶頭部的準(zhǔn)確位置和姿態(tài)提供相應(yīng)的虛擬圖像．在室內(nèi)精密導(dǎo)航系統(tǒng)[7]中,車輛的位置和方向是智能停車系統(tǒng)所需要的基本信息．航天體的對接也需要精確的航天器姿態(tài)和相對位置信息．

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

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

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

1 單基站基于波達(dá)角度的剛體定位

框架

本節(jié)將介紹基于DOA測量的單基站RBL框架．為了確定三維空間中剛體的狀態(tài)(包括位置和姿態(tài)),在其表面安裝無線節(jié)點作為特征點,形成一個小規(guī)模的無線傳感器網(wǎng)絡(luò)．此傳感網(wǎng)絡(luò)的拓?fù)湫畔⑹孪仁峭耆赖?并且在移動和旋轉(zhuǎn)時保持不變．

方便起見,將單基站位置設(shè)置為原點．單基站上配備了一個二維天線陣列進(jìn)行測量來自傳感網(wǎng)中每個節(jié)點的信號到達(dá)的方向．為了描述無線傳感器網(wǎng)絡(luò)當(dāng)前的狀態(tài),需要預(yù)先定義一個已知的參考幀(幀由各個節(jié)點的三維位置構(gòu)成的)．當(dāng)前幀,即當(dāng)前剛體的狀態(tài),假設(shè)是通過參考幀旋轉(zhuǎn)和平移得到(如圖1所示),在數(shù)學(xué)上表示為

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

(1)

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

(2)

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

傳感器網(wǎng)絡(luò)的拓?fù)湫畔⑹蔷_已知的,且在旋轉(zhuǎn)平移中不變,那么可以用節(jié)點對的歐氏距離來描述此拓?fù)湫畔?/p>

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

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

K,i

(3)

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

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

(4)

其中

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

(5)

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

然估計

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

2.1 拓?fù)淦ヅ渥畲笏迫还烙?/h3>

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

首先使用投影角作為量測,是為了簡化當(dāng)前幀中的節(jié)點坐標(biāo)ek和θk之間的關(guān)系:

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

(6)

如此一來,當(dāng)前幀的3×K個三維坐標(biāo)未知數(shù)減少為1×K個未知數(shù),即

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

然后利用無線節(jié)點的確切已知拓?fù)湫畔建立最大似然函數(shù)．d和待估計值φ1的關(guān)系為

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

(7)

(8)

(9)

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

實質(zhì)上,拓?fù)淦ヅ渥畲笏迫还烙嬍峭ㄟ^獲取K傳感器的ey坐標(biāo)來實現(xiàn)RBL的,其匹配信息是K(K-1)/2個距離．而待估計的參數(shù)個數(shù)為K個ey,k,故K>3為估計成功的必要條件．

2.2 測量匹配最大似然估計

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

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

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

(10)

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

(11)

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

(12)

(13)

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

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

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

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

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

高斯-牛頓迭代法的基本思想是用泰勒級數(shù)展開法逼近非線性回歸模型,然后經(jīng)過多次迭代,多次修改回歸系數(shù),使回歸系數(shù)不斷逼近非線性回歸模型的最佳回歸系數(shù),最后使原模型的殘差平方和最?。鲜鰞煞N最大似然估計問題可以表示為

(14)

(15)

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

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

(16)

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

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

(17)

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

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

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

4 性能評估

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

(18)

作為剛性目標(biāo)的先驗狀態(tài)信息,相對于初始幀,當(dāng)前幀位于x∈[0,9]∩y∈[0,9]∩z∈[0,5] m的三維空間,單BS置于原點．同時,三個旋轉(zhuǎn)角度從[0，2π]rad進(jìn)行隨機設(shè)置．

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

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

橫向來看,基于DOA測量匹配最大似然估計方案的魯棒性優(yōu)于基于拓?fù)湫畔⑵ヅ浞桨?前者的收斂成功率始終保持在95%以上,即使在最惡劣的情況下(當(dāng)D=1 m時最高噪聲水平σ=1 deg)．這是一個較為出色的性能,因為牛頓迭代法對于初值是相當(dāng)敏感的,在改進(jìn)的方案中,通過初值重置,基于DOA測量匹配最大似然估計也能具有良好的性能,而拓?fù)淦ヅ浞桨傅氖諗砍晒β嗜匀惠^低．直觀地說,從高斯-牛頓法的角度來說,匹配條件越多(在K=4個節(jié)點的情況下,測量匹配方案的8個條件,包括8個DOA測量值;拓?fù)淦ヅ浞桨傅?個條件,包括6個節(jié)點對距離),測量匹配最大似然估計方案對測量噪聲的魯棒性越強,因此DOA測量匹配最大似然估計方案性能高于拓?fù)淦ヅ洌畧D3也給出了RBL方法的計算成本．在一般意義上,待估計參數(shù)的維數(shù)越高,收斂速度越慢．然而可以看出基于測量匹配最大似然估計方案計算速度卻更加有效．這是因為其較高的收斂成功率減少了牛頓算法中初始值重置的次數(shù)．相反,拓?fù)淦ヅ浞桨革@然需要最大的計算資源．還應(yīng)注意到的是,DOA測量匹配最大似然估計方案在低級測量噪聲下是有效的,而隨著噪聲的增加,其效率會降低．

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

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

圖4和圖5展示了所有估計方案在D=1和D=3時的RBL精確度．本文選擇拓?fù)淦ヅ浞桨钢械氖諗砍晒Y(jié)果計算RMSE,但與克拉美羅界相比仍有明顯的差距,特別是對于小型剛體．這與上面的分析保持一致,即使用的匹配條件越多,性能越好．DOA匹配方案利用了更多的匹配條件,在較高的DOA噪聲水平下可以達(dá)到克拉美羅界,但與克拉美羅界的性能差距越來越大．此現(xiàn)象可能是由于旋轉(zhuǎn)角度與旋轉(zhuǎn)矩陣之間轉(zhuǎn)換過程中的高度非線性造成的．

DOA噪聲標(biāo)準(zhǔn)差/deg(a)平移矢量t估計

DOA噪聲標(biāo)準(zhǔn)差/deg(b)放置矩陣Q估計圖4 當(dāng)D=1 m時RBL估計精度比較

DOA噪聲標(biāo)準(zhǔn)差/deg(a)平移矢量t估計

DOA噪聲標(biāo)準(zhǔn)差/deg(b)旋轉(zhuǎn)矩陣Q估計圖5 當(dāng)D=3 m時RBL估計精度比較

5 結(jié)論和展望

本文提出了基于DOA的單基站RBL框架的兩種最大似然估計方法,一是通過匹配拓?fù)湫畔砉烙嫯?dāng)前幀中節(jié)點的坐標(biāo),然后估計位置姿態(tài),稱為拓?fù)淦ヅ渥畲笏迫还烙嫹桨?二是利用DOA測量直接求出最優(yōu)的平移向量和旋轉(zhuǎn)角度,稱為DOA測量匹配最大似然估計方案．采用改進(jìn)的牛頓-高斯算法對兩個MLE函數(shù)進(jìn)行優(yōu)化,根據(jù)先驗信息對初值進(jìn)行有規(guī)律的重置,以降低收斂失敗率．最后,從收斂成功率、計算代價和精度方面對兩種估計方案在不同情況下進(jìn)行評估．結(jié)果表明,在不同情況下,第二種方案可以在保持95%以上的收斂成功率的同時,在高噪聲情況下其RMSE近似于克拉美羅界．

這項工作只考慮到拓?fù)湫畔⑹蔷_已知的,當(dāng)剛性目標(biāo)來自第三方時,可能無法滿足這一要求．因此,在未來的研究中,只需大致了解RBL的拓?fù)浣Y(jié)構(gòu),便可以研究噪聲拓?fù)湫畔BL性能的影響．在完成RBL任務(wù)的同時,只利用DOA測量和粗糙拓?fù)湫畔υ肼曂負(fù)湫畔⑦M(jìn)行校正,將是一個有趣的研究課題．