大部件對接中基于單位四元數(shù)的iGPS測量位姿比對研究

徐碧菡， 孫涪龍， 趙 罡， 邢宏文

(1.北京航空航天大學中法工程師學院，北京 100191；2.北京航空航天大學機械工程及自動化學院，北京 100191；3.上海飛機制造有限公司，上海 200436)

iGPS (indoor GPS)是一種用于大尺寸空間測量與定位的新型三坐標工業(yè)測量系統(tǒng)，其工作原理類似于GPS，利用三角測量原理建立三維坐標系，不同的是采用紅外激光代替了衛(wèi)星(微波)信號。該系統(tǒng)具有測量范圍大、精度高、實時性好、系統(tǒng)安裝方便等特點[1]，并且支持多用戶，即可以使用多個傳感器來完成不同的測量任務，如部件定位、指導機器人工作及同步跟蹤[2]等，具有廣闊的發(fā)展前景。目前國內(nèi)各大航空航天企業(yè)已經(jīng)開始引進iGPS，但對其在飛機裝配過程中的應用仍然處于探索階段。

在大尺寸部件數(shù)字化裝配的過程中，運用iGPS進行合理布局，可較為便捷地測得部件關(guān)鍵點的位置信息，此時最為關(guān)鍵的問題即為對所測數(shù)據(jù)進行處理并解算出部件的位姿。但由于iGPS實測數(shù)據(jù)所采用的坐標系統(tǒng)為測量系統(tǒng)定義的局部三維直角坐標系，與理論模型所采用的設(shè)計坐標系進行統(tǒng)一后，實測數(shù)據(jù)點與理論模型并不一致，因此針對iGPS所測數(shù)據(jù)與理論模型進行位姿比對研究是進一步利用iGPS輔助部件位姿調(diào)整的重要前提。

在測量數(shù)據(jù)與理論模型進行比對的過程中，需要首先實現(xiàn)剛體姿態(tài)解算。目前用于剛體姿態(tài)解算的方法主要有：奇異值分解法、三點法、最小二乘法、七參數(shù)法、單位四元數(shù)法等[3-5]。其中，奇異值分解法雖然求解結(jié)果較為準確，然而其應用前提為假設(shè)質(zhì)點經(jīng)過坐標轉(zhuǎn)換后完全重合，實際在大部件對接中，由于測量誤差的存在，質(zhì)點經(jīng)過坐標轉(zhuǎn)換后不完全重合，從而影響精度；三點法只適用于不共線的3個點，不適用于iGPS同時測量多個關(guān)鍵點的使用條件；最小二乘法精度受初始值影響大[3]；七參數(shù)法在實際使用過程中涉及針對較小旋轉(zhuǎn)角度的線性化[4]，因此若旋轉(zhuǎn)角度較大，就會產(chǎn)生較大的誤差，不適宜用于大部件對接過程中的剛體位姿描述；單位四元數(shù)法描述剛體的旋轉(zhuǎn)與平移變換直觀明了，可針對多點進行計算[5]，并且具有無奇點、不涉及大量三角函數(shù)計算等優(yōu)點，具備作為大部件對接過程中剛體姿態(tài)描述方法的條件。

本文根據(jù)iGPS本身測量數(shù)據(jù)的特點，對測量數(shù)據(jù)的不同形式進行分析與選擇，并根據(jù)多點同時測量、大旋轉(zhuǎn)角等測量要求，提出一種基于單位四元數(shù)的測量數(shù)據(jù)與理論模型位姿比對計算方法，最后通過實驗對該算法進行了分析與驗證。

1 大部件對接中的iGPS測量

iGPS由激光發(fā)射器、傳感器、手持探頭、接收器電路和系統(tǒng)軟件組成。測量時，發(fā)射器產(chǎn)生兩個激光平面在工作區(qū)域內(nèi)旋轉(zhuǎn)，轉(zhuǎn)速約為3000 r/min。接收器根據(jù)其所能接收到的激光信號，對水平角與垂直角進行測量[6]。

iGPS的測量數(shù)據(jù)以Frame為單位。Frame與坐標系類似，每個Frame可包含6個自由度信息，即3個方位自由度信息與3個位置自由度信息。但根據(jù)測量前用戶對Frame中所包含傳感器個數(shù)及相對位置的不同設(shè)定，F(xiàn)rame的自由度信息也會不同。例如，僅包含由單個傳感器返回數(shù)據(jù)的Frame，只包含點位信息，即3個位置自由度信息；包含兩個相距固定距離的傳感器所返回數(shù)據(jù)的Frame具有5個自由度信息；而包含4個相對位置固定的傳感器(且其中至少3個傳感器不共線)所返回數(shù)據(jù)的Frame，則包含6個自由度信息，即由iGPS底層數(shù)據(jù)處理軟件Surveyor擬合得出的Frame整體的位置與姿態(tài)信息。

使用iGPS進行對大部件上多個測量點進行數(shù)據(jù)測量時，根據(jù)Frame的不同數(shù)據(jù)結(jié)構(gòu)，有兩種數(shù)據(jù)形式可以選擇，分別為點位測量與位姿測量。點位測量僅包含3個位置自由度信息。當對多個測量點分別布置單個傳感器，并將每個傳感器所測數(shù)據(jù)設(shè)定為單獨的Frame時，得到的測量數(shù)據(jù)是每個傳感器幾何中心的位置坐標。而位姿測量則需將Frame設(shè)置為由多個相互位置固定的傳感器(且至少3個傳感器不共線)及其所測數(shù)據(jù)構(gòu)成的整體，得到的數(shù)據(jù)是由Surveyor經(jīng)過數(shù)據(jù)融合計算得出的Frame坐標系原點位置和三坐標軸信息，以及各個點在Frame坐標系下的坐標，但無法獲取每個傳感器所測的原始數(shù)據(jù)。

在實際測量過程中，點位測量與位姿測量分別有各自的優(yōu)點與不足。點位測量形式的優(yōu)點為可直接采用測量點的原始數(shù)據(jù)進行計算，并根據(jù)實際情況有針對性地設(shè)計位姿標定算法；缺點為計算過程可能較為復雜。位姿測量形式的優(yōu)點為可以直接方便地獲取對應Frame的位置與姿態(tài)，即測量目標的位置與姿態(tài)；缺點為該位姿標定過程由Surveyor自動完成，計算過程涉及多次數(shù)據(jù)融合，用戶難以針對實際測量情況控制位姿標定過程，誤差分析較難進行。

因此，本文為避免由系統(tǒng)自身多次數(shù)據(jù)融合所帶來的誤差，采用點位測量形式來配置Frame數(shù)據(jù)結(jié)構(gòu)，即測量各點的位置坐標信息(3自由度)，并以此為基礎(chǔ)設(shè)計適用于飛機對接過程的模型位姿比對算法，為后續(xù)的部件對接提供輸入值。

2 基于單位四元數(shù)的iGPS測量數(shù)據(jù)位姿比對算法

將所測點位數(shù)據(jù)導入理論模型設(shè)計軟件后，iGPS測量坐標系與理論模型的全局坐標系重合，便可獲知在同一坐標系下，兩對接部件各自的理論位姿與實際位姿。此時應首先求解部件實測點位信息與理論模型對應點位信息之間的對應關(guān)系，即求解兩組一一對應點之間的坐標轉(zhuǎn)換關(guān)系。根據(jù)前文對目前幾種常用姿態(tài)解算方法的分析，本文采用單位四元數(shù)法描述部件姿態(tài)。

2.1 基于單位四元數(shù)的坐標轉(zhuǎn)換方法

設(shè)初始點集為A，目標點集為E，其中目標點與參考點數(shù)目相同、一一對應。若每個點集中點數(shù)為k，則可記A={ai}，E={ei}，i=1,2,…,k。計算兩組一一對應點的坐標轉(zhuǎn)換關(guān)系，即為求解滿足：

的最佳旋轉(zhuǎn)矩陣R與平移向量T。

根據(jù)剛體定點轉(zhuǎn)動理論中的歐拉定理：剛體繞固定點的任一位移，都可由繞過此點的某一軸轉(zhuǎn)過一個角度得到。假設(shè)坐標系OXYZ下該旋轉(zhuǎn)矩陣對應剛體在三維空間中繞單位轉(zhuǎn)軸向量v=(ax,ay,az)旋轉(zhuǎn)θ角，則基于單位四元數(shù)的坐標轉(zhuǎn)換方法思想[6]如下：

假設(shè)旋轉(zhuǎn)轉(zhuǎn)換向量為單位四元數(shù)

其中各分量分別為[7]：

且各分量滿足：

則根據(jù)單位四元數(shù)可得到3×3的旋轉(zhuǎn)矩陣R(qR)[8-9]：

設(shè)平移向量為：

則完整的坐標變換向量即為：

考慮到飛機部件尺寸大剛性差的特點，難以做到坐標轉(zhuǎn)換后與模型完全重合，根據(jù)式(1)，結(jié)合最小二乘原理設(shè)計優(yōu)化函數(shù)如下：

至此求解對應點集間的最佳坐標轉(zhuǎn)換向量問題轉(zhuǎn)化為求使得f(q)具有最小值的坐標轉(zhuǎn)換向量q。

分別求出初始點集A與目標點集E的重心：

由集合A與集合E構(gòu)成協(xié)方差矩陣：

然后利用反對稱矩陣：

的輪換對稱項來構(gòu)成列向量：

并進一步構(gòu)造用于求解最優(yōu)旋轉(zhuǎn)變換向量的矩陣Q(ΣAE)[8]：

其中，I3為3×3的單位矩陣，tr(ΣAE)為矩陣ΣAE的跡。對應Q(ΣAE)取最大特征值的特征向量qR=[qqqq]T即為最優(yōu)的旋轉(zhuǎn)變換向量。因此，最優(yōu)的平移向量為：

至此，旋轉(zhuǎn)矩陣與平移向量求解完畢，即可實現(xiàn)坐標轉(zhuǎn)換。旋轉(zhuǎn)矩陣所對應的旋轉(zhuǎn)軸v與旋轉(zhuǎn)角度值θ可結(jié)合式(3)計算得出。

2.2 iGPS測量數(shù)據(jù)與理論模型比對方法

假設(shè)理論模型中，給定兩對接部件M、N，其中M為固定部件，N為移動部件。M與N分別對應于實際部件M′與N′。測量時分別在M′與N′上選定并布置m與n個測量點(m≥3，n≥3)，m與n之間無相互約束關(guān)系，可視實際測量需求而定。

首先考慮固定部件，由于其理論模型M在設(shè)計坐標系下所有坐標點的信息已知，則可獲取設(shè)計坐標系下m個測量點的理論坐標信息；實測部件M′上所布置的m個測量點的坐標信息通過iGPS實測獲得。此時便有了兩組一一對應的坐標值，可采用2.1節(jié)中的基于單位四元數(shù)的轉(zhuǎn)換方法計算其轉(zhuǎn)換關(guān)系Q。此處涉及的轉(zhuǎn)換為將理論模型M上的m個點轉(zhuǎn)換到實測部件位置。

其次考慮移動部件，確定在實際部件N′上的n個測量點后，通過iGPS實測獲取相對應的n個坐標點信息。此時由于根據(jù)第一步中求出的轉(zhuǎn)換關(guān)系Q，即可對移動部件理論模型N上的n個坐標點進行同樣的坐標變換，從而求出實際測量場地中移動部件N′的目標位置N′′。

至此確定了移動部件的實際位置N′與目標位置N′′，即可再一次運用2.1節(jié)中所介紹的基于單位四元數(shù)的坐標轉(zhuǎn)換方法，計算出移動部件到達目標位置所需的坐標轉(zhuǎn)換關(guān)系P，從而為后續(xù)調(diào)姿步驟提供輸入數(shù)據(jù)。

該方法的具體實現(xiàn)流程如圖1所示。

圖1 iGPS測量數(shù)據(jù)與理論模型比對總體方法計算流程

3 實驗與分析

3.1 驗證實驗方案

為驗證本文提出的基于單位四元數(shù)的iGPS測量數(shù)據(jù)位姿比對算法的精確度，設(shè)計驗證實驗。整體思路為，首先根據(jù)兩對接部件實測點位信息與理論點位信息計算相關(guān)坐標轉(zhuǎn)換關(guān)系；接著在理論模型中兩部件對接面上取3個既屬于固定部件又屬于移動部件的點，用于驗證，理想情況即為：計算得到的移動部件目標位姿上的這3個點，與固定部件實測位姿上的這3個點基本重合；于是對這3個點分別應用第一步擬合得出的坐標變換關(guān)系，得出固定部件上此3點的實際位置信息，和移動部件上此3點的目標位置信息，計算這兩組位置信息之間的誤差，考察2節(jié)中所提出算法的精度。

具體實驗過程如下：

(1) 首先為實際固定部件與移動部件分別選取m個與n個測量點；

(2) 根據(jù)2.1節(jié)中坐標轉(zhuǎn)換方法，由固定部件實測點位信息M′與其理論點位信息M，計算M向M′的轉(zhuǎn)換關(guān)系Q；

(3) 根據(jù)式(3)計算Q中單位四元數(shù)對應的旋轉(zhuǎn)軸vQ與旋轉(zhuǎn)角度θQ；

(4) 根據(jù)2.1節(jié)中坐標轉(zhuǎn)換方法，由移動部件實測點位信息N'與其理論點位信息N，計算由N向N′的轉(zhuǎn)換關(guān)系S；

(5) 根據(jù)式(3)計算S中單位四元數(shù)對應的旋轉(zhuǎn)軸vS與旋轉(zhuǎn)角度θS；

(6) 對移動部件理論點位信息N應用轉(zhuǎn)換關(guān)系Q，得到移動部件目標點位信息N′′；

(7) 根據(jù)2.1節(jié)中坐標轉(zhuǎn)換方法，由移動部件實測點位信息N′與移動部件目標點位信息N′′，計算固定部件由N′向N′′的轉(zhuǎn)換關(guān)系P；

(8) 根據(jù)式(3)計算P中單位四元數(shù)對應的旋轉(zhuǎn)軸vP與旋轉(zhuǎn)角度θP；

(9) 對移動部件實測點位信息N′應用轉(zhuǎn)換關(guān)系P，得到移動部件最終點位信息N′′′；

(10) 在理論模型中兩部件對接面上取3個驗證點，則這3個點既屬于固定部件，又屬于移動部件。理想條件下對接后固定部件和移動部件上的這3個點應當重合。將此3點位置信息記為MNTest；

(11) 對固定部件對接面上的3個點應用變換關(guān)系Q，得到固定部件驗證點的實際位置信息MTest；

(12) 對移動部件對接面上的3個點應用變換關(guān)系S，使其到達移動部件實測位置N′附近，得出移動部件驗證點的實際位置信息NTest；

(13) 對NTest的3個點應用變換關(guān)系P，得到移動部件驗證點的目標位置信息OTest；

(14) 對比MTest與OTest兩組驗證點的偏差。計算偏差平均值

與標準差σTest。

3.2 實驗數(shù)據(jù)

下面根據(jù)3.1節(jié)中的實驗方案進行3組實驗。

為實際固定部件與移動部件分別選取4個測量點，即m=n=4。(實際應用過程中，應根據(jù)實際測量點的選取情況確定m與n的取值。)

固定部件理論模型上4個對應點位信息M如表1所示。

移動部件理論模型上4個對應點位信息N如表2所示。

表2 移動部件理論模型對應點位信息N

實驗結(jié)果如表3所示。計算過程中涉及的各坐標轉(zhuǎn)換關(guān)系所對應的旋轉(zhuǎn)軸、旋轉(zhuǎn)角度、旋轉(zhuǎn)矩陣以及平移向量結(jié)果也均在表中呈現(xiàn)。

表3 驗證實驗結(jié)果

(待續(xù))

(續(xù)表)

(待續(xù))

(續(xù)表)

3.3 結(jié)果分析

由實驗結(jié)果可以看出，在第I組與第Ⅲ組實驗中，固定部件的實際點位信息不變，僅改變移動部件的實際點位信息，因此移動部件由理論模型點位信息向?qū)崪y點位信息轉(zhuǎn)動角度以及由實測點位信息向目標點位信息轉(zhuǎn)動角度都不同，平移向量也不同，但最終驗證點之間的誤差都小于10-11數(shù)量級。

第Ⅱ組實驗與第I組相比，則將固定部件與移動部件的實際位置同時改變，但可以看出，無論是大旋轉(zhuǎn)角還是小旋轉(zhuǎn)角，坐標轉(zhuǎn)換的誤差始終小于10-11數(shù)量級，標準差也小于10-11數(shù)量級。

4 結(jié) 論

本文針對大部件對接過程中iGPS多點同時測量、對接旋轉(zhuǎn)角大的特點，提出了基于單位四元數(shù)的iGPS測量數(shù)據(jù)位姿比對算法，用于部件對接過程中測量數(shù)據(jù)與理論模型的位姿比對。該算法描述位姿變換簡潔直觀，計算量小，效率高。誤差計算與驗證實驗結(jié)果表明，該算法對旋轉(zhuǎn)角大小和平移量無特殊要求，且在不同條件下均有很高的轉(zhuǎn)換精度，能有效適用于iGPS的測量位姿比對計算。

[1]呂景亮,張春富,唐文彥,郝欣宇.Indoor GPS 工業(yè)測量系統(tǒng)自標定技術(shù)研究[J].計量學報,2011,32(1):12-15.

[2]Norman A R,Sch?nberg A,Gorlach I A,Schmitt R.Validation of iGPS as an external measurement system for cooperative robot positioning [J].The International Journal of Advanced Manufacturing Technology,2013,64(1-4): 427-446.

[3]羅 芳,鄒 方,周萬勇.飛機大部件對接中的位姿計算方法[J].航空制造技術(shù),2011,(3): 91-94.

[4]陳 宇,白征東.基于非線性最小二乘算法的空間坐標轉(zhuǎn)換[J].大地測量與地球動力學,2010,30(2):129-132.

[5]Horn B K P.Closed-form solution of absolute orientation using unit quaternions [J].JOSA A,1987,4(4): 629-642.

[6]吳曉峰,張國雄.室內(nèi)GPS測量系統(tǒng)及其在飛機裝配中的應用[J].航空精密制造技術(shù),2006,42(5): 1-5.

[7]Yoon D,Narduzzi M,Shen J.Computing rotation minimizing frames using quaternions [J].Computer-Aided Design and Applications,2012,9(5):679-690.

[8]Besl P J,McKay N D.A Method for registration of 3-D shapes [J].IEEE Transactions on Pattern Analysis and Machine Intelligence,1992,14(2): 239-256.

[9]滕志遠,張愛武.單位四元素法在激光點云坐標轉(zhuǎn)換中的應用[J].測繪通報,2010(11): 7-10.