基于C#編程實現(xiàn)高斯牛頓法求解激光跟蹤干涉儀基站空間坐標

唐文秀，林 虎，薛 梓，秦海濛，鐵咪咪

(1.北京信息科技大學 儀器科學與光電工程學院，北京 100192；2.中國計量科學研究院，北京 100029；3.天津大學 精密儀器與光電子工程學院，天津 300072)

1 引 言

隨著大型裝備制造業(yè)的發(fā)展，在船舶、軌道交通、航空航天等領域都對大尺寸測量技術及測量設備提出了越來越高的要求。激光跟蹤儀是大尺寸測量中常用的測量設備，可用于精密幾何測量和三維運動軌跡跟蹤，具有測量范圍大、速度快、精度高等優(yōu)點[1]。近些年，各種新型的激光跟蹤儀相繼被研制出來，其中德國物理技術研究院和英國國家物理實驗室共同研制了一種新型激光跟蹤干涉儀[2]，采用球度誤差小于50 nm的標準球作為參考反射鏡，該標準球固定在熱膨脹系數(shù)極小的殷鋼支柱上，干涉測量系統(tǒng)僅圍繞該固定標準球轉動，因此機械結構對于精度的影響可以降低到很小，并且只利用干涉測量長度進行空間坐標的解算，避免了角度測量引入的誤差，具有很高的精度[3,4]。其中基于多邊法的四路激光跟蹤三維坐標測量技術是近年的研究熱點[5～9]。四路激光跟蹤三維坐標測量系統(tǒng)在實際應用前首先需要進行系統(tǒng)多參量自標定，確定出跟蹤干涉儀的基站坐標。系統(tǒng)多參量的自標定過程本質是一個非線性最優(yōu)化問題，求解非線性最優(yōu)化問題常用的方法有高斯牛頓法、Levenberg-Marquart法、信賴域法等等[10]。高斯牛頓法由于避免了計算大規(guī)模的Hessian矩陣所帶來的不便，極大地簡化了運算量，實際應用廣泛。因此，本文通過C#與MATLAB混合編程、C#語言編程兩種方式實現(xiàn)高斯牛頓法求解激光跟蹤干涉儀基站空間坐標值。

2 激光跟蹤干涉儀基站空間坐標標定原理

如圖1所示為激光跟蹤干涉儀基站空間坐標標定原理圖，對于多臺激光跟蹤干涉儀組成三維坐標測量系統(tǒng)，其每個基站坐標標定原理類似。在全局坐標系統(tǒng)中，激光跟蹤干涉儀第j個基站坐標為(x0j,y0j,z0j)(j=1,…,4)，對于任意空間目標點Pi(xi,yi,zi)，激光跟蹤干涉儀可以測出其相對初始目標點P0(x0,y0,z0)的相對長度變化量Δlij=lij-l0j，這里需要求解基站坐標及到初始目標點的長度值l0j。為了對上述參量進行標定和求解，需要在全局坐標系統(tǒng)中設置一系列已知的目標點Pi(xi,yi,zi)(i=1,2,…,m)，利用激光跟蹤干涉儀對上述目標點進行依次測量，根據(jù)基站坐標、初始點坐標及目標點坐標的空間幾何關系可以建立如下方程組：

(1)

在上述方程組中，需要求解的未知參量為(x0j,y0j,z0j,l0j)，為了獲取更高的解算精度，需要更多的冗余方程，即目標點的數(shù)量要大于4個，此時方程個數(shù)大于待求參量個數(shù)，構成超定方程組。超定方程組沒有通常意義下的確定解，但是可通過數(shù)值算法解算出近似最優(yōu)解。因此，求解激光跟蹤干涉儀基站空間坐標問題轉化為對非線性超定方程組的最優(yōu)化求解，如何進行最優(yōu)化求解并且通過C#編程實現(xiàn)成為本文研究的關鍵內容。

圖1 激光跟蹤干涉儀基站空間坐標標定原理圖

3 高斯牛頓算法求解原理

(2)

進一步可推導為：

(3)

上述非線性超定方程組具有一定的平方和結構特殊性，屬于小殘量問題，因此，可選用高斯牛頓法求解非線性超定方程組的最優(yōu)解。該算法對于非線性擬合具有較好的擬合精度，收斂速度較快，對初值依賴強，必須合理選取初值，它通過舍棄Hessian矩陣的二階偏導數(shù)實現(xiàn)，可用于數(shù)據(jù)擬合、參數(shù)估計和函數(shù)估計等方面，并且在實際應用中，常用于目標定位、單點定位等坐標求解問題[11～13]。

ri=(b4+Δlij)-

(4)

求解該最小二乘問題即讓目標函數(shù)最小，數(shù)學表達式為：

m>4

(5)

將f(b)按泰勒公式在bk處展開并略去高階項：

f(b)=f(bk)+g(bk)T(b-bk)+

(6)

函數(shù)ri(b)二次連續(xù)可微，目標函數(shù)的梯度g(b)與Hessian矩陣G(b)分別為：

(7)

=J(b)TJ(b)+s(b)

(8)

根據(jù)高斯牛頓迭代法，對ri分別求b1、b2、b3、b4的偏導，其中初值為b0=[b01,b02,b03,b04]，[b01,b02,b03]為根據(jù)空間位置粗略估計激光跟蹤干涉儀初始坐標，b04為激光跟蹤干涉儀到初始動點的迭代初始值，構成的雅克比矩陣J如下所示：

(9)

將g(b)與G(b)代入式(6)可得：

(10)

忽略Hessian矩陣的二階偏導s(bk)后，根據(jù)牛頓法推導可得高斯牛頓迭代公式為：

bk+1=bk-(JTJ)-1JTr(bk)

(11)

4 C#與MATLAB混合編程關鍵技術

為了實現(xiàn)最優(yōu)解的快速迭代求解，可直接在科學計算軟件MATLAB中調用相關函數(shù)。MATLAB具有豐富的函數(shù)庫，專門用于數(shù)值分析，調試簡單，但不擅長于后續(xù)測控軟件開發(fā)。為了求解出激光跟蹤干涉儀基站空間坐標，并且顯示到用戶界面中，方便用戶進行數(shù)據(jù)分析處理，本文考慮采用C#與MATLAB混合編程，由C#實現(xiàn)用戶界面設計，MATLAB實現(xiàn)最優(yōu)解的快速迭代求解，C#與MATLAB采用.NET組件技術實現(xiàn)混合編程，在MATLAB中調用lsqnonlin函數(shù)并編譯生成動態(tài)鏈接庫(Dynamic Link Library，DLL)文件，在C#中添加該.DLL文件引用，即可在C#中執(zhí)行與MATLAB同樣的計算功能[14～18]。具體實現(xiàn)步驟如下：

(1)在MATLAB中新建一個.m文件，運行編譯工具(deploytool)，生成動態(tài)鏈接庫。

(2)打開C#，添加該引用以及程序集文件“MWArray.dll”并加入命名空間，MWArray是用于C#與MATLAB之間的數(shù)據(jù)交換類。

(3)在C#中進行數(shù)據(jù)類型轉換，調用動態(tài)鏈接庫開始計算，并將結果返回到界面中顯示。

C#與MATLAB混合編程的方式可充分發(fā)揮兩種語言與平臺的優(yōu)勢，特別是調用MATLAB的數(shù)學函數(shù)能保證迭代算法的正確性，但是混合編程的方式也存在其缺點：一是運行程序之前必須提前安裝整個MATLAB軟件，準備工作比較繁瑣，而最終只用到其中的幾個函數(shù)，當程序需要商業(yè)化時，其經(jīng)濟性就凸顯不足；二是每次運算時，數(shù)據(jù)需要頻繁地在兩個平臺之間傳輸，當測量點數(shù)量較多時，程序的運行速度大大減慢，也不利于軟件界面的開發(fā)。為解決上述不足，本文進一步研究了僅依靠C#語言編程來實現(xiàn)高斯牛頓法求解激光跟蹤干涉儀基站空間坐標，并通過與MATLAB混合編程的結果比較，驗證C#編程算法的準確性。

5 C#實現(xiàn)高斯牛頓算法

5.1 數(shù)據(jù)定義

定義已知的m個目標點的空間坐標，l為激光跟蹤干涉儀的測距讀數(shù)，b為待求參量(x0j,y0j,z0j,l0j)T，設置迭代次數(shù)為20次，精度因子為10-8。

5.2 設計類進行矩陣運算

設計一個類為Matrix用于矩陣的運算，包含矩陣的加法、減法、乘法、轉置以及求逆，并且在計算之前進行是否符合矩陣運算的規(guī)則判斷。

5.3 重復迭代計算

根據(jù)激光跟蹤干涉儀在三維空間的位置估計，給出一個粗略的迭代初值，開始迭代循環(huán)計算，并最終得到滿足精度因子的解。

第1步：按式(4)定義殘差公式數(shù)學模型；

第2步：求出雅克比矩陣J及其轉置矩陣JT，其中ri對b1、b2、b3、b4的偏導分別為：

(12)

第3步：求出的矩陣JTJ，以及殘差ri；

第4步：計算(JTJ)-1JTri，代入迭代公式bk+1=bk-(JTJ)-1JTr(bk)；

6 實驗驗證

如圖2所示，利用三坐標測量機及激光跟蹤干涉儀搭建實驗系統(tǒng)，激光目標靶鏡裝在三坐標測量機的測頭座上。在該實驗系統(tǒng)中，三坐標測量機的機器坐標系為全局空間坐標系，激光跟蹤干涉儀的基站空間坐標是未知的，三坐標測量機的坐標讀數(shù)為激光目標靶鏡的坐標值，當激光目標靶鏡在全局空間坐標系內運動時，激光跟蹤干涉儀能夠跟蹤目標靶鏡并進行長度測量。

圖2 激光跟蹤干涉儀基站空間坐標標定實驗

利用上述實驗系統(tǒng)對激光跟蹤干涉儀開展基站空間坐標標定實驗。首先將激光跟蹤干涉儀置于第1個站位LT1上，令激光目標靶鏡在全局坐標系中運動至設定好的6個目標點，測量每個目標點相對初始目標點的相對長度變化值；然后將激光跟蹤干涉儀陸續(xù)移動至第2個站位LT2和第3個站位LT3，在每個站位下令激光目標靶鏡運動至不同的6個目標點，分別采集激光跟蹤干涉儀的數(shù)據(jù)如表1所示。

表1 空間目標點坐標及激光跟蹤干涉儀測距讀數(shù)

將表1中數(shù)據(jù)分別導入到C#的兩個程序代碼中，第一個程序代碼為C#與MATLAB混合編程，第二個為C#編程，編程軟件平臺為微軟Visual Studio 2017。

在兩個程序中分別對激光跟蹤干涉儀的基站空間坐標進行優(yōu)化迭代計算和求解，迭代初值為每個基站站位的估計值，基站站位的精確計算結果以及兩種方式的運行時間如表2所示。

表2 C#編程與C#調用MATLAB計算結果

從表2列出的激光跟蹤干涉儀3個基站空間坐標計算結果可以看到：采用C#編程的高斯牛頓法在迭代5或6次后即與C#調用MATLAB的計算結果一致，差值在10-7數(shù)量級；并且從運行時間來看，用時更少，效率更高。實驗結果表明C#編程實現(xiàn)了高斯牛頓算法對非線性超定方程組最優(yōu)解的快速求解，驗證了利用C#編程求解激光跟蹤干涉儀基站空間坐標的準確性以及可行性。

7 結 論

本文將激光跟蹤干涉儀空間坐標標定問題轉化為非線性最小二乘問題，采用C#編程實現(xiàn)了高斯牛頓算法。通過實驗將C#編程計算結果與C#調用MATLAB的計算結果相比，差值在10-7數(shù)量級，并且效率更高，驗證了C#編程算法的正確性以及可行性，說明了高斯牛頓法進行非線性擬合具有較好的擬合精度，收斂速度較快，為后續(xù)進行多個站位下的聯(lián)合測量以及利用多邊法原理反求動點運動軌跡研究打下了基礎。