經緯儀最佳一致逼近多項式速度實時求解

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

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

1 引言

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

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

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

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

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

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

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

2 測速工作原理

2.1 位置測量

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

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

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

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

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

最小二乘法數據處理方法如下：

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

設Y1，Y2，…，Yn的估計值為y1，y2，…，yn，則關系式如式（3）所示：

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

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

若數據l1，l2，…，ln的測量誤差是服從正態分布，設定標準差為σ1，σ2，…，σn，則各測量結果l1，l2，…，ln出現于真值附近dδ1，dδ2，…，dδn區域內的概率為：

根據最大或然原理，可認為這n個測量值同時出現于相應區間dδ1，dδ2，…，dδn時概率最大，應滿足：

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

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

相應的外推估計量為：

其誤差方程為：

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

2.3 目標測速模型建立

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

圖2 目標測速示意圖Fig.2 Schematic diagram of target velocity measurement

根據圖2 的幾何關系可以推出目標從P1飛到P2位移如式（12）所示：

目標速度公式如式（13）所示：

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

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

3 速度求解

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

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

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

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

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

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

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

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

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

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

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

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

引入記號Qm(x)：

其中m≤n。

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

速度誤差估計為：

為了減少運算量，設：

取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 有限差分的方法識別速度的野值

設函數表中的節點xi等間距分布：xi=x0+ih，fi為相應的函數值，h稱為步長。差fi+1-fi稱為一階差分，用Δfi表示。m階差分函數值表示：

當x≡x0+ih時，等式（39）成立：

當xi≤ζ≤xi+m時，

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

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

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

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

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

3.3 迭代求解

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

3.4 測速工作流程圖

測速工作步驟：

（1）輸入經緯儀數據A，E；

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

（3）單站計算目標位置；

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

（5）根據速度模型計算速度；

（6）剔野值、濾波保存速度數據。

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

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

3.5 實驗結果比對

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

3.5.1 激光測得距離濾波實驗

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

4 結論

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