基于小二乘－卡爾曼濾波的wMPS系統(tǒng)跟蹤定位算法研究*

端木瓊，楊學(xué)友，邾繼貴

(天津大學(xué)精密測試技術(shù)及儀器國家重點實驗室，天津300072)

隨著現(xiàn)代裝備制造業(yè)、空間技術(shù)和機器人技術(shù)的發(fā)展，靜態(tài)測量已越來越不能滿足部分現(xiàn)代工業(yè)的要求，一些航空、航天及高精密制造企業(yè)的客戶希望對其生產(chǎn)或使用的大型工件以及整套系統(tǒng)進行動態(tài)測量。在工業(yè)測量領(lǐng)域，動態(tài)測量的主要方法有激光跟蹤儀測量技術(shù)以及攝影測量技術(shù)，但是這些方法都有其局限性。攝影測量需要通視條件，視場較小，而激光跟蹤儀只能實現(xiàn)單點跟蹤測量，姿態(tài)測量則需增加其它附件，在環(huán)境復(fù)雜的現(xiàn)場條件下容易受到干擾［1］。wMPS是一種基于光電掃描的網(wǎng)絡(luò)式大尺寸測量系統(tǒng)，與其它系統(tǒng)相比，該系統(tǒng)具有測量精度高、并行測量能力強、抗干擾能力強等特點。本文在靜態(tài)測量的基礎(chǔ)［2－6］上對系統(tǒng)的動態(tài)測量原理進行了研究，將靜態(tài)測量與卡爾曼濾波相結(jié)合，實現(xiàn)了動態(tài)過程中的精密定位，減小了運動導(dǎo)致的測量誤差，對大型設(shè)備裝配過程中的跟蹤定位測量具有重要意義。

1 wMPS系統(tǒng)組成

wMPS三維定位系統(tǒng)主要由激光發(fā)射器網(wǎng)絡(luò)、位置傳感器、中心計算機和無線通訊系統(tǒng)組成［2］。激光發(fā)射器由固定基座和轉(zhuǎn)動頭組成，安裝有兩個一字線激光器和一個脈沖激光器，兩個一字線激光器固定于轉(zhuǎn)動頭上，激光器產(chǎn)生的光平面分別與垂直方向呈±30°，呈V字形。當(dāng)發(fā)射器工作時，激光器所產(chǎn)生的光平面隨轉(zhuǎn)動頭一同旋轉(zhuǎn)，對測量空間進行掃描。脈沖激光器用于產(chǎn)生一個計時同步時刻，以該時刻光平面1與發(fā)射器水平面之間的交線為發(fā)射器X正方向，旋轉(zhuǎn)軸為Z方向，按右手定則確定Y方向。當(dāng)系統(tǒng)工作時，在工件裝配的特定工作空間中的不同區(qū)域放置多個發(fā)射器(Transmitter)，接收器(Receiver)安裝在工件的關(guān)鍵點上，由于發(fā)射器與接收器之間的光信號通信是單向廣播式(One-Way Broadcast-Style)的，因此多個接收器可以共用這些發(fā)射器信號，如圖1所示。傳感器通過計算與兩個(以上)發(fā)射器的方位角，通過空間交會原理即可得到傳感器的空間三維坐標(biāo)［2－4］。

圖1 wMPS三維坐標(biāo)系統(tǒng)組成

2 基于最小二乘原理的靜態(tài)定位方法

wMPS系統(tǒng)的靜態(tài)定位通常采用基于多面交會的最小二乘估計方法［7－9］。如圖2(a)所示，發(fā)射器可以抽象為圍繞旋轉(zhuǎn)軸以角速度ω旋轉(zhuǎn)的兩個光平面。以初始時刻，光平面1與發(fā)射器水平面之間的交線為發(fā)射器X正方向，旋轉(zhuǎn)軸為Z方向，按右手定則確定Y方向。轉(zhuǎn)臺位于初始位置時，對兩個光平面進行標(biāo)定，得到兩個光平面的方程為:

即

圖2 wMPS系統(tǒng)測量原理

若被測點P的坐標(biāo)為(x，y，z)，則可得方程組

對于多個發(fā)射器，將其方程組寫成矩陣形式可得

其最小二乘解為

利用最小二乘法解算接收器的坐標(biāo)只利用了當(dāng)前的觀測量，不能對觀測量進行誤差分析，因此解算結(jié)果受觀測量影響較大，當(dāng)觀測量誤差較大或接收器運動時，解算結(jié)果精度不高，但最小二乘法收斂速度很快，受初始位置誤差影響較小。

3 基于最小二乘－卡爾曼濾波的動態(tài)測量方法

如果直接利用接收器測得的與多個發(fā)射器之間的方位角信息進行卡爾曼濾波跟蹤定位，不可避免要解決非線性估計問題，這將使算法復(fù)雜，同時引入非線性誤差，降低解算精度。此外，隨著觀測量維數(shù)的增加，計算量也大幅增加，因此我們利用靜態(tài)坐標(biāo)解算的方法對接收器坐標(biāo)進行估計，將其結(jié)果作為偽測量值，然后在利用勻速運動模型進行線性卡爾曼濾波，實現(xiàn)高精度定位跟蹤［10－12］。

3．1 運動方程和測量方程

在數(shù)字化裝配過程中，工件所做的多為勻速運動、勻加速運動以及低速轉(zhuǎn)彎等機動性較弱的運動，因此選用勻速運動模型建立運動方程，將加速度作為運動噪聲。設(shè)狀態(tài)變量 X=［x，y，z，vx，vy，vz］T，其中 x，y，z分別為接收器的空間三維坐標(biāo)，vx，vy，vz分別為接收器運動速度在x，y，z方向上的分量，采樣間隔為T。狀態(tài)轉(zhuǎn)移方程為:

其中，狀態(tài)轉(zhuǎn)移矩陣、狀態(tài)轉(zhuǎn)移噪聲矩陣分別為

狀態(tài)轉(zhuǎn)移噪聲向量為

同時，狀態(tài)轉(zhuǎn)移噪聲滿足

系統(tǒng)的觀測量 分別為接收器的坐標(biāo)(x，y，z)，觀測方程為

其中，觀測矩陣為

觀測噪聲滿足

3．2 線性卡爾曼濾波

采用最小二乘計算所得的坐標(biāo)作為偽觀測量，系統(tǒng)轉(zhuǎn)移方程和觀測方程均為線性，采用卡爾曼濾波對測量結(jié)果進行最優(yōu)化估計。卡爾曼濾波步驟如下:

(1)計算狀態(tài)變量及其協(xié)方差初值。k=1，k=2時，直接利用觀測值Z1，Z2通過最小二乘原理計算出接收器的位置分別為(x1，y1，z1)和(x2，y2，z2)。求出系統(tǒng)狀態(tài)變量初始值為

(2)利用狀態(tài)轉(zhuǎn)移方程及前一步的狀態(tài)變量對當(dāng)前狀態(tài)進行最優(yōu)估值。當(dāng)k≥3時，系統(tǒng)狀態(tài)變量Xk|k－1和方差 Pk|k－1的先驗估計為:

(3)通過觀測量對估值進行修正。殘余增益K及后驗估計Xk|k和后驗方差Pk|k分別為:

(4)將后驗估計Xk|k和Pk|k作為初值代入第2步，再次進行濾波。

4 試驗驗證

為了驗證最小二乘－卡爾曼定位跟蹤方法的有效性，并與靜態(tài)跟蹤方法進行比較，本文在精密測試技術(shù)及儀器國家重點實驗研制的wMPS原型平臺上進行了驗證，試驗平臺如圖3所示。

圖3 wMPS系統(tǒng)實驗平臺

試驗采用了四個發(fā)射站，分別布置在工作區(qū)域的四個角上，工作區(qū)域約為8 m×6 m×2 m，測量前利用Lecia激光跟蹤儀作為基準(zhǔn)對wMPS系統(tǒng)進行了標(biāo)定，標(biāo)定方法與文獻［7］相同。接收器在測量空間中分別沿x、y、z方向做勻速移動，移動速度分別為 0．05 m/s和 0．2 m/s，測量采樣頻率為 20 Hz。

圖4給出了接收器以速度0．05 m/s做勻速直線運動時利用四個發(fā)射器數(shù)據(jù)跟蹤估值曲線。在測量數(shù)據(jù)質(zhì)量較好，數(shù)據(jù)冗余較多的情況下，最小二乘－卡爾曼濾波法的測量誤差小于±2 mm，效果遠(yuǎn)好于最小二乘法估值。

圖4 0．05 m/s勻速直線運動時估值誤差對比

圖5給出了接收器以速度0．2 m/s勻速直線運動時的測量結(jié)果。測量結(jié)果表明，隨著運動速度的加快，兩種估值方法的測量誤差都有所增大，最小二乘－卡爾曼濾波法的估值誤差增加至±3 mm，而最小二乘法的估值誤差增大到±10 mm。

圖5 0．2 m/s勻速直線運動時估值誤差對比

5 結(jié)論

本文將卡爾曼濾波引入到了wMPS系統(tǒng)的動態(tài)跟蹤中來，并與靜態(tài)坐標(biāo)估算想結(jié)合，克服了采用掃描角進行濾波帶來的非線性誤差及計算量增大的問題。試驗結(jié)果表明，最小二乘－卡爾曼濾波法在測量誤差較大、可利用發(fā)射站數(shù)量較少的情況下仍然能夠較好的跟蹤目標(biāo)，跟蹤精度始終好于2 mm，能夠滿足工業(yè)數(shù)字化裝配的應(yīng)用，具有良好的應(yīng)用前景。

［1］ 張靚，李明，郁菁菁，等．物體空間動態(tài)測量方法的研究［J］．機械制造，2007，45(12):54－56．

［2］ 端木瓊，楊學(xué)友，邾繼貴，等．基于光電掃描的網(wǎng)絡(luò)式大尺寸測量系統(tǒng)定位算法研究［J］．傳感技術(shù)學(xué)報，2011，24(9):1－5．

［3］ Yang Linghui，Yang Xueyou，Zhu Jigui，et al，Novel Method for Spatial Angle Measurement Based on Rotating Planar Laser Beams［J］．Chinese Journal of Mechanical engineering，2010，23(6):758－764．

［4］ 楊凌輝，楊學(xué)友，勞達寶，等．采用光平面交匯的大尺寸坐標(biāo)測量方法［J］．紅外與激光工程，2010，39(6):1829－1833．

［5］ 勞達寶，楊學(xué)友，邾繼貴，等．網(wǎng)絡(luò)式激光掃描空間定位系統(tǒng)標(biāo)定技術(shù)研究［J］．機械工程學(xué)報，2011，47(6):1829－1833．

［6］ 楊凌輝，楊學(xué)友，邾繼貴，等．基于光電掃描的工作空間測量定位系統(tǒng)誤差分析［J］．光電子·激光，2010，21(12):1829－1833．

［7］ 邾繼貴，王鑫，王大為，等．光學(xué)坐標(biāo)測量系統(tǒng)技術(shù)研究［J］．傳感技術(shù)學(xué)報，2007，20(4):778－780．

［8］ 張滋黎，邾繼貴，耿娜，等．雙經(jīng)緯儀三維坐標(biāo)測量系統(tǒng)設(shè)計［J］．傳感技術(shù)學(xué)報，2010，23(5):660－664．

［9］ 李廣云．非正交系坐標(biāo)測量系統(tǒng)原理及進展［J］．測繪信息與工程，2003，28(1):4－10．

［10］邱玲，沈振康．三維純角度被動跟蹤定位的最小二乘－卡爾曼濾波算法［J］．紅外與激光工程，2001，30(2):83－86．

［11］宋驪平，姬紅兵，高新波．多站測角的機動目標(biāo)最小二乘自適應(yīng)跟蹤算法［J］．電子與信息學(xué)報，2005，27(5):793－796．

［12］宋驪平，姬紅兵．多站測角的最小二乘交互多模型跟蹤算法［J］．西安電子科技大學(xué)學(xué)報，2008，35(2):242－247．