多套光學經緯儀聯合跟蹤的容錯最小距離平方和定位

梁家輝，李 建，胡紹林，3*

（1. 西安理工大學 自動化與信息工程學院，陜西 西安710048；2. 中國兵器工業(yè)試驗測試研究院，陜西 華陰714200；3. 廣東石油化工學院 自動化學院，廣東 茂名525000）

1 引 言

經緯儀是一種根據測角原理設計的測量俯仰角和方位角的測量儀器，通常有光學經緯儀和電子經緯儀兩種類型。1730 年光學經緯儀最早由英國機械師Sisson 研制，1904 年德國開始生產玻璃度盤經緯儀。隨著電子技術的發(fā)展，20 世紀60 年代出現了電子經緯儀。光學經緯儀的水平度盤和堅直度盤均用光學玻璃制成，電子經緯儀采用編碼度盤、光柵度盤、電柵度盤或記時測角度盤，實現了數字化自動測角。

無論是光學經緯儀還是電子經緯儀，都是用于測定遠處目標位置，廣泛應用于大地測量、地圖繪制、雷達標校、飛機校飛、導航制導、火箭跟蹤及衛(wèi)星軌道測定等領域［1］。

經緯儀定位的基本原理是利用兩臺（或多臺）經緯儀分別測定目標相對于經緯儀軸心位置的俯仰角E 和方位角A，利用多組測角數據（A，E），采用幾何交會算法或最小二乘法，逐點計算目標點在參照系下的坐標，實現目標位置解算。經典的交會算法主要有K-公式法、L-公式法或最小二乘法［1-4］，以及最小一乘等改進方法。K-公式和L-公式雖有簡潔的解析表達式，但是只用到部分數據信息，精度受限；最小二乘或最小一乘法，雖用到全部測量數據，但涉及非線性方程求解，不便于數據快速計算，且對野值數據缺乏容錯能力；基于L-公式提出的4 臺經緯儀兩兩交會測量結果的加權融合處理方法［5］，雖然精度較高，但不適用于三站或多站的交會測量；文獻［6］和［7］對該方法進行了改進，可適用于兩站和三站交會測量，但隨目標飛行高度的增加，基于L-公式的融合處理方法的誤差不斷增大，性能會迅速下降；HOU 等人提出了基于兩個相交平面公垂線的異面直線法［8］，SU 等人對K-公式、L-公式法和異面直線法精度做了比較，證實3 種方法在不同適用區(qū)的定位精度差異較大［9］。

本文提出一組基于最短距離和的多臺站經緯儀測角數據融合容錯定位方法。該方法直觀、簡單，對野值數據具有容錯能力，通過求解線性方程組即可得到目標軌跡坐標，能實現對于全部測角數據的融合，不受野值數據的影響。

2 基于最短距離和的融合定位

圖1 經緯儀測量數據的幾何關系示意圖Fig. 1 Geometric relationship of theodolite measurement data

假設測定目標M 在任意t 時刻的空間位置，記經緯儀在t 時刻的測量數據為( t，A，E )。由于單臺經緯儀的任一組數據都不可能單獨確定出M的位置，即使測量數據是理想值，也只能確定t時刻飛行器在射線Li上某個位置，如圖1 所示。

如果有N臺經緯儀同時跟蹤該目標，可假設目標M在t時刻應該同時在圖2 所示的N條射線上，即M應該在這N條射線的交點上。但是，實際中測量數據中不可避免地含有誤差，誤差的存在使得這N條射線未必相交于一點。這N條射線最多可以相交于C2N個不同點，當然也有可能沒有交點，這就使得常規(guī)的幾何交會方法失效。不過，在沒有任何偏好信息（也即事先認定哪臺經緯儀的測量數據有更可靠的先驗知識）的情況下，可以認為飛行器最可能的位置應該處于和這N條射線距離最短的點上。

2.1 目標點M 到經緯儀主光軸的距離

假設有N臺經緯儀同時跟蹤目標M，為便于利用經緯儀的觀測數據解算M的位置，需要構建公共坐標系，如圖2 所示。

構建公共坐標系O-XYZ，各經緯儀設備旋轉中心Oi在O-XYZ坐標系下的坐標為（X0i，Y0i，Z0i），記 其 與 經 緯 儀 固 有 測 站 坐 標 系Oi-XiYiZi之間轉換關系的旋轉矩陣是Ri，則不難導出經緯儀i的主光軸在O-XYZ下的直線方程為：

圖2 多經緯儀聯合觀測示意圖Fig. 2 Multi theodolite joint observation

利用空間解析幾何知識，目標點M（X，Y，Z）到第i臺經緯儀的主光軸直線Li的距離可以表示為：

2.2 M 點坐標的最優(yōu)估計

為融合N臺經緯儀的測量數據，盡可能準確地解算目標M的三維坐標，設想M點位于與這N臺經緯儀主光軸距離和最小的位置上。由式（2）可以得出目標M到N臺經緯儀各自主光軸的距離和，即：

式中：

對于式（3）所表示的極值問題，采用費馬引理［10］得到：

由于式（4）是非線性方程組，求顯示解困難，本文將式（3）轉換為距離平方和：

式中：

由三元一次線性方程組式（6），可寫出（X，Y，Z）的系數行列式：

顯然，C 關于主對角線對稱且主對角線上元素不為0，所以C ≠0，則由克萊姆法則［11］可知方程組式（6）有唯一解，且解為：

3 算法改進

多年靶場外測數據處理工作的實踐表明，即使是高精度的測量設備，由于多種偶然因素的綜合影響或作用，采樣數據集合中往往包含（0. 1～0. 2）%的嚴重偏離目標真值的異常數據［1］。工程領域稱這部分異常數據為野值。野值對測角數據交會解算目標位置有著十分不利的影響。因此，本文提出一種自適應容錯算法，可在不剔除野值的情況下得到可靠的交會結果。算法流程如圖3 所示，具體步驟如下：

（1）假設N臺經緯儀獲取M在tj( j= 1，2，3，...)時刻的測角數據( A( tj)，E ( tj))，在t時刻之前數據無野值，由式（8）計算目標定位坐標，記為：

（2）采用多項式擬合外推法［12］對t 時刻之前的定位結果一步外推，得到t 時刻預測結果，記為X^t；

（3）構造Hampel 型［13］函數：

式中c為3～5 倍的σ，σ為測角數據標準差。

（4）利用X^t反算目標相對于N臺經緯儀的測角數據，得到反算的方位角和俯仰角數據，記為(A^ti，E^ti)；

（5）基于反算的目標相對各經緯儀的方位角和俯仰角數據(A^ti，E^ti)，以及式（10）所示的φ函數，構造二維數據：

圖3 容錯定位算法處理流程Fig. 3 Processing flow chart of fault-tolerant location algorithm

4 仿真計算

為驗證基于最短距離和的融合定位算法的有效性，并準確評估其性能，本文采用某飛行試驗任務部分真實航跡數據作為原始數據。該數據是以地面監(jiān)控中心某點為公共坐標系原點，采樣間隔為0. 25 s，隨時間變化的三維坐標數據序列(t，X，Y，Z)如圖4 所示。

圖4 真實航跡Fig. 4 Real track

已知3 臺經緯儀測站的大地坐標，通過坐標轉換將真實航跡數據反演至3 個經緯儀測站系下，得到各站測角數據(t，Ai，Ei)，部分數據如表1所示。

考慮到大地測量誤差、大氣折射誤差、地球曲率半徑等不影響本文算法的有效性，仿真中忽略上述誤差的影響。采用Monte Carlo 方法生成均方差為5″的正態(tài)分布隨機誤差序列，疊加到表1 所示的經緯儀方位角數據和仰角數據上，形成隨機誤差符合正態(tài)分布的經緯儀測角數據序列。

利用本文基于最短距離和的融合定位法，以及上述帶有正態(tài)隨機誤差的3 臺經緯儀測角數據，計算目標航跡(t，X′，Y′，Z′)，結果如圖5 所示。為了直觀地顯示算法精度，逐點計算融合定位結果與真實航跡數據的殘差(t，ΔX，ΔY，ΔZ)，各方向殘差如圖6 所示。

由圖5～圖6 可知，3 個方向的定位結果除疊加的隨機誤差引起的擾動外，目標3 個方向的坐標計算值與真實航跡（圖4）完全一致，不存在系統(tǒng)性偏差，從而證明了融合定位算法的有效性。

表1 各站測角數據Tab. 1 Angle measurement data of each station

圖5 基于最短距離和的融合定位結果Fig. 5 Result of location fusion based on sum of shortest distance

圖6 融合結果與真實航跡殘差Fig. 6 Fusion results and real track residual

5 容錯仿真計算

對形成的隨機誤差符合正態(tài)分布的經緯儀測角數據序列，采用外部疊加方法形成偏置量不同 的 野 值 點 ：（1）A1( 80 )=A1( 80 )- 0.04，（2）A2( 81 )=A2( 81 )- 0.04，（3）E1(180 )=E1(180 )+0.04，（4）A3( 250 )=A3( 250 )- 0.3，（5）E2( 350 )=E2( 350 )+ 0.3，（6）A4( 530 )=A4( 530 )-0.085，（7）E3( 530 )=E3( 530 )+ 0.085，仿真數據如圖7 和圖8 所示。

對圖7 和圖8 所示的帶多個野值點的仿真數據，分別進行無容錯能力的融合定位和有容錯能力的融合定位，定位結果分別如圖9和圖10所示。

圖7 含隨機誤差和野值的三站方位角數據Fig. 7 Azimuth data of three stations including random error and outliers

圖8 含隨機誤差和野值的三站俯仰角數據Fig. 8 Pitch angle data of three stations including random error and outliers

從圖9 和圖10 可以看出，3 個方向的定位結果除疊加的隨機誤差引起的擾動外，無容錯能力的融合定位算法得到的結果在各個野值點處都有非常大的偏差，說明野值會嚴重影響定位算法結果的可靠性；而容錯定位算法得到的結果與不含野值的計算結果（圖5）基本一致，不存在系統(tǒng)偏差，算法具有較好的抗野值能力。

圖9 無容錯定位結果Fig. 9 Non fault-tolerant positioning results

圖10 容錯定位結果Fig. 10 Fault-tolerant positioning results

6 結 論

本文建立的最小距離和容錯定位算法既能夠充分利用測量數據信息，又可以在不進行野值檢驗的情況下進行定位計算，確保定位結果不失真。仿真結果表明，該方法對測量數據具有良好的容錯能力。當測量數據正常時定位結果達到距離和最小；當測量數據含孤立型野值或長度不超過3 個的斑點型野值時，無需事前修復即可確保定位結果可靠，幾乎不受野值影響，能夠保證動態(tài)目標的實時可靠定位。