多套光學(xué)經(jīng)緯儀聯(lián)合跟蹤的容錯最小距離平方和定位

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

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

1 引 言

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

無論是光學(xué)經(jīng)緯儀還是電子經(jīng)緯儀，都是用于測定遠(yuǎn)處目標(biāo)位置，廣泛應(yīng)用于大地測量、地圖繪制、雷達(dá)標(biāo)校、飛機校飛、導(dǎo)航制導(dǎo)、火箭跟蹤及衛(wèi)星軌道測定等領(lǐng)域［1］。

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

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

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

圖1 經(jīng)緯儀測量數(shù)據(jù)的幾何關(guān)系示意圖Fig. 1 Geometric relationship of theodolite measurement data

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

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

2.1 目標(biāo)點M 到經(jīng)緯儀主光軸的距離

假設(shè)有N臺經(jīng)緯儀同時跟蹤目標(biāo)M，為便于利用經(jīng)緯儀的觀測數(shù)據(jù)解算M的位置，需要構(gòu)建公共坐標(biāo)系，如圖2 所示。

構(gòu)建公共坐標(biāo)系O-XYZ，各經(jīng)緯儀設(shè)備旋轉(zhuǎn)中心Oi在O-XYZ坐標(biāo)系下的坐標(biāo)為（X0i，Y0i，Z0i），記 其 與 經(jīng) 緯 儀 固 有 測 站 坐 標(biāo) 系Oi-XiYiZi之間轉(zhuǎn)換關(guān)系的旋轉(zhuǎn)矩陣是Ri，則不難導(dǎo)出經(jīng)緯儀i的主光軸在O-XYZ下的直線方程為：

圖2 多經(jīng)緯儀聯(lián)合觀測示意圖Fig. 2 Multi theodolite joint observation

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

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

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

式中：

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

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

式中：

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

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

3 算法改進(jìn)

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

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

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

（3）構(gòu)造Hampel 型［13］函數(shù)：

式中c為3～5 倍的σ，σ為測角數(shù)據(jù)標(biāo)準(zhǔn)差。

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

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

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

4 仿真計算

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

圖4 真實航跡Fig. 4 Real track

已知3 臺經(jīng)緯儀測站的大地坐標(biāo)，通過坐標(biāo)轉(zhuǎn)換將真實航跡數(shù)據(jù)反演至3 個經(jīng)緯儀測站系下，得到各站測角數(shù)據(jù)(t，Ai，Ei)，部分?jǐn)?shù)據(jù)如表1所示。

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

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

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

表1 各站測角數(shù)據(jù)Tab. 1 Angle measurement data of each station

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

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

5 容錯仿真計算

對形成的隨機誤差符合正態(tài)分布的經(jīng)緯儀測角數(shù)據(jù)序列，采用外部疊加方法形成偏置量不同 的 野 值 點 ：（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，仿真數(shù)據(jù)如圖7 和圖8 所示。

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

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

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

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

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

圖10 容錯定位結(jié)果Fig. 10 Fault-tolerant positioning results

6 結(jié) 論

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