趙立榮，袁光福，吳 冬，高 群，王瀟洵

（1.中國科學(xué)院 長春光學(xué)精密機械與物理研究所，吉林 長春 130033；2.中國人民解放軍95859部隊，甘肅 酒泉 735000）

1 引言

光電經(jīng)緯儀廣泛用于火箭、導(dǎo)彈和無人機等飛行目標(biāo)的外彈道/航跡參數(shù)測量，是靶場主測設(shè)備之一，主要進行目標(biāo)極坐標(biāo)測量即方位角、高低角。測量目標(biāo)速度的應(yīng)用相對較少，武器實驗靶場一般采用連續(xù)波脈沖雷達的多普勒效應(yīng)來實現(xiàn)對被試目標(biāo)的速度進行測量，該方法可以獲得較高的速度測量精度。隨著數(shù)據(jù)擬合、濾波等數(shù)據(jù)處理技術(shù)的發(fā)展，對空中目標(biāo)進行測速的應(yīng)用越來越多。帶激光測距的經(jīng)緯儀系統(tǒng)通過對空中目標(biāo)進行光學(xué)偵察以獲取目標(biāo)相對于地面站的方位和俯仰信息；然后，利用激光測距獲取目標(biāo)的距離信息，對目標(biāo)進行空間定位；最后，通過多次定位數(shù)據(jù)和定位時間間隔，計算得到目標(biāo)的飛行速度。經(jīng)緯儀獲得的位置信息在時間序列上是離散的，在對毫秒級采樣間隔時間差分求速度時，經(jīng)緯儀位置上一點小的誤差，會造成很大的速度誤差，因此對測量數(shù)據(jù)的濾波、擬合、平滑及外推是實時高精度獲得速度信息的關(guān)鍵［1-5］。

王冰等人在文獻［6］中提出經(jīng)緯儀單站測量速度，通過多次定位數(shù)據(jù)和定位時間間隔，計算得到目標(biāo)的飛行速度。飛行速度49～56 m/s，誤差為3%～16%，此方法最小速度誤差為1.47 m/s，誤差較大。

應(yīng)用在經(jīng)緯儀上擬合目標(biāo)運動軌跡求速度的方法，大多采用最小二乘法，最小二乘法是根據(jù)均方根誤差最小的原理得到目標(biāo)的運動軌跡求速度。對于做復(fù)雜運動，且速度較快的目標(biāo)來說，要達到較高的擬合精度，擬和函數(shù)的階次較高，計算量大，實時性不好。三次樣條函數(shù)插值求速度，具有二階光滑度和良好的收斂性，但是沒有對三階毛刺誤差進行處理，在實際測量速度時，會存在毛刺誤差［7-8］。

文獻［9］提出一種基于雷達和光學(xué)經(jīng)緯儀合理布站優(yōu)化組合測量與數(shù)學(xué)計算求取彈丸速度參數(shù)的方法，采用三次Hermite 函數(shù)表示，優(yōu)點是計算精度穩(wěn)定，缺點誤差大。

多年靶場外測數(shù)據(jù)處理工作的實踐表明，即使是高精度的測量設(shè)備，也會因為多種偶然因素的影響，采樣數(shù)據(jù)集合中往往包含0.1%～0.2%的嚴(yán)重偏離目標(biāo)真值的異常數(shù)據(jù)［10］。工程領(lǐng)域稱這部分異常數(shù)據(jù)為野值。野值對目標(biāo)測量速度有十分不利的影響，需要采用有效的方法進行剔除。

利用經(jīng)緯儀測量目標(biāo)速度，通常獲得目標(biāo)位置，然后通過差分獲得目標(biāo)速度，比較典型的求解方法有高斯函數(shù)方法及卡爾曼方法。高斯函數(shù)方法求解速度實時性好，但測量速度精度不高；卡爾曼方法求速度精度很好，但是因為用了大量的歷史數(shù)據(jù)，速度值產(chǎn)生滯后。只有提高速度的實時性、高精度才能滿足靶場上導(dǎo)彈、飛機等多種目標(biāo)測速要求。

本文采用最佳一致逼近多項式的方法對測量速度進行擬合、平滑及外推，保證了速度的實時性；采用了有限差分的方法識別速度的野值，去除野值，確定去除野值閾值，利用迭代的方法，獲得最優(yōu)速度值。本文方法既保證了目標(biāo)測量速度的實時性，又保證了目標(biāo)測量速度精度，在實際經(jīng)緯儀加裝激光測距系統(tǒng)中，取得了很好的效果。

2 測速工作原理

2.1 位置測量

單站光電經(jīng)緯儀通過加裝激光測站儀，獲得目標(biāo)三個測量元素：距離R、方位角A及俯仰角E。以經(jīng)緯儀中心點OC為原點，Xc軸方向為大地北，建立OC(XC，YC，ZC)坐標(biāo)系，如圖1 所示。

圖1 單站定位原理示意圖Fig.1 Schematic diagram of single station positioning principle

單站定位公式如式（1）所示：

2.2 最小二乘方法剔除激光測距距離野值

經(jīng)緯儀激光測距機測得的距離首先進行擬合剔除野值，再與方位角、高低角計算位置，從而計算的速度才能更精確。最小二乘法實時性不好，采用數(shù)據(jù)外推的方式改進最小二乘法，提高距離實時性［11］。

最小二乘法數(shù)據(jù)處理方法如下：

確定t個測量的參數(shù)X1，X2，…，Xt的估計量x1，x2，…，xt，測量值Y，存在t個未知值與Y是函數(shù)關(guān)系，n次測量Y值，獲得l1，l2，…，ln數(shù)據(jù)，有關(guān)系式如式（2）所示：

設(shè)Y1，Y2，…，Yn的估計值為y1，y2，…，yn，則關(guān)系式如式（3）所示：

而l1，l2，…，ln的殘余誤差公式如式（4）和式（5）所示：

式（4）和式（5）為殘余誤差方程式。

若數(shù)據(jù)l1，l2，…，ln的測量誤差是服從正態(tài)分布，設(shè)定標(biāo)準(zhǔn)差為σ1，σ2，…，σn，則各測量結(jié)果l1，l2，…，ln出現(xiàn)于真值附近dδ1，dδ2，…，dδn區(qū)域內(nèi)的概率為：

根據(jù)最大或然原理，可認(rèn)為這n個測量值同時出現(xiàn)于相應(yīng)區(qū)間dδ1，dδ2，…，dδn時概率最大，應(yīng)滿足：

結(jié)果為估計量，以殘余誤差的形式表示，即：

式（8）存在平方量，求解困難，可以把其線性化。利用級數(shù)展開把式（8）非線性形式線性地表達出來：

相應(yīng)的外推估計量為：

其誤差方程為：

最后根據(jù)誤差最小擬合曲線，求解平滑后的距離值。此方法的精度取決于誤差閾值，設(shè)定誤差越小，精度越高，計算量越大。

2.3 目標(biāo)測速模型建立

利用激光測距獲取目標(biāo)的距離信息，對目標(biāo)進行空間定位，通過擬合獲得準(zhǔn)確的彈道位置數(shù)據(jù)，定位時間間隔已知，計算得到目標(biāo)的飛行速度［6］。如圖2 所示，以經(jīng)緯儀中心點OC為原點，Xc軸方向 為大地 北，建 立OC(XC，YC，ZC)坐 標(biāo)系，設(shè)在t1和t2時刻目標(biāo)從P1飛到P2，線速度為v，兩點相對于OC點的坐 標(biāo)分別 為P1(A1，E1，R1，t1)和P2(A2，E2，R2，t2)，A1、E1、R1為P1點對應(yīng)的方位角、俯仰角及目標(biāo)截距，同理A2，E2，R2為P2點對應(yīng) 的方位 角、俯仰角 及目標(biāo)截距；P1'和P2'為P1點和P2點在 地面的投 影。

圖2 目標(biāo)測速示意圖Fig.2 Schematic diagram of target velocity measurement

根據(jù)圖2 的幾何關(guān)系可以推出目標(biāo)從P1飛到P2位移如式（12）所示：

目標(biāo)速度公式如式（13）所示：

由速度測量公式（13）可知，目標(biāo)速度v是一個與目標(biāo)截距、方位角及俯仰角等參數(shù)有關(guān)的函數(shù)，可以表示為v(A1，A2，E1，E2，R1，R2)。其引起誤差的原因有：（1）光測設(shè)備激光測距在P1點和P2點的激光測距誤差δ(R1)，δ(R2)；（2）光測設(shè)備目標(biāo)在在P1點和P2點測角誤差δ(A1)，δ(A2)，δ(E1)及δ(E2)。

速度的誤差公式可表示為：

3 速度求解

3.1 最佳一致逼近多項式計算速度函數(shù)

計算速度的函數(shù)通常采用多項式表達，多項式采用常規(guī)的表達形式會產(chǎn)生很大的計算誤差，不能很好地逼近數(shù)據(jù)真值，為了減少計算誤差，多項式采用逼近性好的切比雪夫多項式組合的方式獲得最佳一致逼近多項式計算速度函數(shù)［12］。

切比雪夫多項式Tn(x)(n≥0)，定義為：

由公式（15）～公式（17）推得：

切比雪夫多項式組合而成的多項式Qn(x)是連續(xù)函數(shù)fn(x)的最佳一致逼近多項式的充分必要條件是在區(qū)間[a，b]上至少存在n+2 個點x0＜…＜xn+1使得：

其中，i=0，…，n+1，對于所有的i同時α=1（或者α=-1）。

點x0，x1，…，xn+1常常稱 為切比 雪夫交替點。

將速度函數(shù)f(x)按照切比雪夫正交函數(shù)系展開成級數(shù)：

低次項速度組合函數(shù)如式（22）所示：

式（22）能確定較好逼近。dj很難顯式計算，卻比較容易進行泰勒展開：

設(shè)定速度函數(shù)多項式Pn(x)使得公式（24）的誤差足夠小：

按照切比雪夫多項式展開Pn(x)：

引入記號Qm(x)：

其中m≤n。

所有多項式Qm(x)都是Qm+1(x)的m階最佳一致逼近多項式，因此：

速度誤差估計為：

為了減少運算量，設(shè)：

取y=2x，vn-1=yvn+Dn-1，按照遞推公式vk=yvk+1-vk+2+Dk獲得Pn(x)：

本文采用了簡單改進切比雪夫公式，采樣4幀速度值分別為d0，d1，d2及d3，計算第5 幀速度值v，為了計算簡便，x取值-1。

3.2 有限差分的方法識別速度的野值

設(shè)函數(shù)表中的節(jié)點xi等間距分布：xi=x0+ih，fi為相應(yīng)的函數(shù)值，h稱為步長。差fi+1-fi稱為一階差分，用Δfi表示。m階差分函數(shù)值表示：

當(dāng)x≡x0+ih時，等式（39）成立：

當(dāng)xi≤ζ≤xi+m時，

可以得到推論：n次多項式的n次有限差分等于常數(shù)，而更高階差分等于零。

根據(jù)公式（35），對于m階差分，誤差以系數(shù)(-1)j放大。如果函數(shù)足夠光滑，那么其不很高階的差分可能很小。通過比較差分結(jié)果，可以識別包含誤差的函數(shù)值，并對其進行修正。

如果某個值fi包含相對較大的誤差ε，則在三階差分中它一定以ε，-3ε，3ε，-ε的形式出現(xiàn)。

根據(jù)上述分析，因為速度函數(shù)是三次多項式，通過求解三階差分的值，判斷含有誤差的測量值，剔除。三階有限差分去除野值流程圖如圖3 所示。

圖3 三階有限差分去除野值流程圖Fig.3 Flow chart of third-order finite difference outlier removal

3.3 迭代求解

在通過有限差分的方法識別速度的野值后，對剔除野值的測量值重新進行最佳一致逼近多項式擬合計算，獲得的值再進行有限三階差分剔除，反復(fù)迭代，直到誤差小于設(shè)定的閾值，在速度為40 m/s 時，根據(jù)經(jīng)驗速度的誤差的閾值選為0.02 m/s。

3.4 測速工作流程圖

測速工作步驟：

（1）輸入經(jīng)緯儀數(shù)據(jù)A，E；

（2）輸入激光測距值R，剔野值、濾波；

（3）單站計算目標(biāo)位置；

（4）位置剔野值、濾波、擬合、外推；

（5）根據(jù)速度模型計算速度；

（6）剔野值、濾波保存速度數(shù)據(jù)。

速度計算流程圖如圖4 所示。

圖4 速度計算流程圖Fig.4 Speed calculation flowchart

3.5 實驗結(jié)果比對

激光測距光電經(jīng)緯儀在外場進行無人機跟飛實驗，設(shè)定200 s 飛行時間，飛行最遠(yuǎn)距離4 000 m，記錄從4 000 m 返航圖像，勻速飛行速度控制在40 m/s。無人機上裝有測速GPS，可以進行速度比對。

3.5.1 激光測得距離濾波實驗

設(shè)備的激光測距機的距離誤差指標(biāo)為均方差小于1 m，由于激光測距機在測距過程中受到跟蹤不穩(wěn)、噪聲、天氣和環(huán)境等因素影響，測得的距離值具有跳動值，如圖5 所示，激光測距獲得的距離值與真值對比，最大誤差為3.449 m。

圖5 距離測量值與真值對比Fig.5 Comparison of distance measurement values with true values

在實驗過程中，必須對距離值進行濾波。通過最小二乘濾波，明顯增強測距的穩(wěn)定性。取目標(biāo)勻速運行段500 幀數(shù)據(jù)進行距離濾波前后對比實驗。對距離濾波前后的值進行一次差數(shù)據(jù)計算，在無人機以40 m/s 勻速運行時，采樣間隔50 ms，一次差理想值為2 m，如圖6 所示距離濾波前一次差幅值范圍-1.4～-2.8 m，濾波后的距離一次差幅值范圍-1.7～-2.4 m。500 幀數(shù)據(jù)距離濾波前一次差幅值的標(biāo)準(zhǔn)偏差0.22，濾波后的距離一次差幅值的標(biāo)準(zhǔn)偏差0.13。

圖6 距離濾波前后值一次差數(shù)據(jù)比較Fig.6 Comparison of first difference data before and after distance filtering

3.5.2 有限差分速度剔野值實驗

常規(guī)野值剔除方法是處理序列數(shù)據(jù)的殘差，對數(shù)據(jù)殘差進行統(tǒng)計，判斷序列數(shù)據(jù)的野值。有限差分剔除野值的方法，數(shù)據(jù)差分計算判斷野值。根據(jù)上述分析，已知速度函數(shù)是三次多項式，通過求解三階差分的值，判斷含有誤差的測量值，剔除野值。

如圖7 所示無人機速度保持40 m/s 勻速飛行，采樣頻率20 Hz。運行時間100 s。第一組實驗數(shù)據(jù)含有兩組小毛刺及兩組大毛刺，第二組實驗數(shù)據(jù)含有三組大毛刺，實驗結(jié)果顯示，大野值完全剔除；去毛刺后的圖線在采樣點200～400 之間、1 000～1 200 之間，相對于去毛刺前的曲線有明顯毛刺凸起，經(jīng)數(shù)據(jù)分析是三階差分的低限值取得過低，人為引入擾動。

圖7 速度去除野值實驗Fig.7 Speed removal outlier test

3.5.3 速度濾波前后與GPS 速度值比對實驗

速度濾波實驗采用1 000 幀數(shù)據(jù)，包括起始過度段，無人機速度保持40 m/s 勻速飛行，采樣頻率20 Hz。運行時間50 s。實時數(shù)據(jù)取五組數(shù)據(jù)帶入最佳一致逼近多項式擬合計算，獲得的值進行有限三階差分剔除后，反迭代，直到誤差小于設(shè)定的閾值，在速度為40 m/s 時，根據(jù)經(jīng)驗速度的誤差的閾值選為0.02 m/s。在測速實驗中，可根據(jù)不同的實時速度，動態(tài)調(diào)整誤差設(shè)定的閾值。速度濾波前后與GPS 差值全部數(shù)據(jù)對比曲線圖如圖8 所示。

圖8 速度濾波前后與GPS 速度全部數(shù)據(jù)對比曲線圖Fig.8 Comparison curve of all data before and after speed filtering with GPS true values

從圖8（b）所示曲線可以看出，濾波后與GPS比對誤差最大誤差小于0.8 m/s，滿足指標(biāo)1 m/s要求。

3.5.4 幾種速度求解方法對比GPS 速度值實驗

幾種求解速度方法對比GPS 速度值實驗如圖9 所示。

圖9 速度求解方法對比GPS 實驗Fig.9 Comparison of Speed Solving Methods with GPS Experiments

如圖9 所示，兩組數(shù)據(jù)都是2 000 幀，無人機速度在33～47 m/s 之間變速飛行，采樣頻率20 Hz。運行時間100 s。全部過程數(shù)據(jù)曲線顯示高斯（Gaussian velocity Solving,GS）有大的毛刺存在；截取尾部數(shù)據(jù)擴大曲線顯示卡爾曼（Kalman filtering speed solution,KLM）數(shù)據(jù)有較大滯后。最佳一致逼近多項式（Optimal Uniform Approximation,OUA）獲得速度與GPS 速度值比對，時許滯后一幀，50 ms，誤差小于0.8 m/s。

4 結(jié)論

本文根據(jù)激光測距經(jīng)緯儀的高精度、實時性的測速要求，提出了最佳一致逼近多項式的測速方法，本方法在實際使用中，主要采用切比雪夫三階多項式來逼近速度真值，利用三階有限差分法剔除速度野值，根據(jù)實時速度，動態(tài)調(diào)整速度迭代誤差閾值，使速度精度更高。本方法獲得速度與GPS 速度值比對，實時性好，延時50 ms；速度精度均方差為0.8 m/s，滿足設(shè)備的指標(biāo)要求。本文采用的有限差分法，也可以應(yīng)用在變化緩慢場景圖像去高階噪聲。