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

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

0 引 言

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

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

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

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

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

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

框架

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

方便起見(jiàn),將單基站位置設(shè)置為原點(diǎn)．單基站上配備了一個(gè)二維天線陣列進(jìn)行測(cè)量來(lái)自傳感網(wǎng)中每個(gè)節(jié)點(diǎn)的信號(hào)到達(dá)的方向．為了描述無(wú)線傳感器網(wǎng)絡(luò)當(dāng)前的狀態(tài),需要預(yù)先定義一個(gè)已知的參考幀(幀由各個(gè)節(jié)點(diǎn)的三維位置構(gòu)成的)．當(dāng)前幀,即當(dāng)前剛體的狀態(tài),假設(shè)是通過(guò)參考幀旋轉(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個(gè)節(jié)點(diǎn)的三維坐標(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)

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

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

圖1 單基站剛體定位測(cè)姿場(chǎng)景

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

K,i

(3)

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

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

(4)

其中

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

(5)

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

然估計(jì)

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

2.1 拓?fù)淦ヅ渥畲笏迫还烙?jì)

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

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

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

(6)

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

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

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

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

(7)

(8)

(9)

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

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

2.2 測(cè)量匹配最大似然估計(jì)

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

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

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

(10)

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

(11)

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

(12)

(13)

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

需要注意的是,通過(guò)分析使用條件和要估計(jì)的參數(shù)的數(shù)量,上述兩個(gè)估計(jì)需要?jiǎng)傮w表面至少綁定三個(gè)或以上的無(wú)線節(jié)點(diǎn),方可來(lái)完成RBL估計(jì)任務(wù)．

3 最大似然估計(jì)高斯-牛頓算法

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

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

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

(14)

(15)

這是一個(gè)非線性最小二乘問(wèn)題．

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

(16)

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

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

(17)

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

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

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

4 性能評(píng)估

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

(18)

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

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

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

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

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

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

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

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

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

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

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

5 結(jié)論和展望

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

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