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

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

1 引 言

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

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

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

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

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

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

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

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

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

假設(shè)有N臺(tái)經(jīng)緯儀同時(shí)跟蹤目標(biāo)M，為便于利用經(jīng)緯儀的觀(guān)測(cè)數(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) 緯 儀 固 有 測(cè) 站 坐 標(biāo) 系Oi-XiYiZi之間轉(zhuǎn)換關(guān)系的旋轉(zhuǎn)矩陣是Ri，則不難導(dǎo)出經(jīng)緯儀i的主光軸在O-XYZ下的直線(xiàn)方程為：

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

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

2.2 M 點(diǎn)坐標(biāo)的最優(yōu)估計(jì)

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

式中：

對(duì)于式（3）所表示的極值問(wèn)題，采用費(fèi)馬引理［10］得到：

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

式中：

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

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

3 算法改進(jìn)

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

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

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

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

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

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

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

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

4 仿真計(jì)算

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

圖4 真實(shí)航跡Fig. 4 Real track

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

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

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

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

表1 各站測(cè)角數(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é)果與真實(shí)航跡殘差Fig. 6 Fusion results and real track residual

5 容錯(cuò)仿真計(jì)算

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

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

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

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

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

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

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

6 結(jié) 論

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