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

端木瓊，楊學友，邾繼貴

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

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

1 wMPS系統組成

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

圖1 wMPS三維坐標系統組成

2 基于最小二乘原理的靜態定位方法

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

即

圖2 wMPS系統測量原理

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

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

其最小二乘解為

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

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

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

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

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

其中，狀態轉移矩陣、狀態轉移噪聲矩陣分別為

狀態轉移噪聲向量為

同時，狀態轉移噪聲滿足

系統的觀測量 分別為接收器的坐標(x，y，z)，觀測方程為

其中，觀測矩陣為

觀測噪聲滿足

3．2 線性卡爾曼濾波

采用最小二乘計算所得的坐標作為偽觀測量，系統轉移方程和觀測方程均為線性，采用卡爾曼濾波對測量結果進行最優化估計。卡爾曼濾波步驟如下:

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

(2)利用狀態轉移方程及前一步的狀態變量對當前狀態進行最優估值。當k≥3時，系統狀態變量Xk|k－1和方差 Pk|k－1的先驗估計為:

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

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

4 試驗驗證

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

圖3 wMPS系統實驗平臺

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

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

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

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

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

5 結論

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

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

［2］ 端木瓊，楊學友，邾繼貴，等．基于光電掃描的網絡式大尺寸測量系統定位算法研究［J］．傳感技術學報，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］ 楊凌輝，楊學友，勞達寶，等．采用光平面交匯的大尺寸坐標測量方法［J］．紅外與激光工程，2010，39(6):1829－1833．

［5］ 勞達寶，楊學友，邾繼貴，等．網絡式激光掃描空間定位系統標定技術研究［J］．機械工程學報，2011，47(6):1829－1833．

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

［7］ 邾繼貴，王鑫，王大為，等．光學坐標測量系統技術研究［J］．傳感技術學報，2007，20(4):778－780．

［8］ 張滋黎，邾繼貴，耿娜，等．雙經緯儀三維坐標測量系統設計［J］．傳感技術學報，2010，23(5):660－664．

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

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

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

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