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

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

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

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

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

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

1 大部件對接中的iGPS測量

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

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

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

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

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

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

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

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

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

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

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

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

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

且各分量滿足：

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

設(shè)平移向量為：

則完整的坐標(biāo)變換向量即為：

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

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

分別求出初始點(diǎn)集A與目標(biāo)點(diǎn)集E的重心：

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

然后利用反對稱矩陣：

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

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

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

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

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

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

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

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

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

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

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

3 實(shí)驗(yàn)與分析

3.1 驗(yàn)證實(shí)驗(yàn)方案

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

具體實(shí)驗(yàn)過程如下：

(1) 首先為實(shí)際固定部件與移動(dòng)部件分別選取m個(gè)與n個(gè)測量點(diǎn)；

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

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

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

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

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

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

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

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

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

(11) 對固定部件對接面上的3個(gè)點(diǎn)應(yīng)用變換關(guān)系Q，得到固定部件驗(yàn)證點(diǎn)的實(shí)際位置信息MTest；

(12) 對移動(dòng)部件對接面上的3個(gè)點(diǎn)應(yīng)用變換關(guān)系S，使其到達(dá)移動(dòng)部件實(shí)測位置N′附近，得出移動(dòng)部件驗(yàn)證點(diǎn)的實(shí)際位置信息NTest；

(13) 對NTest的3個(gè)點(diǎn)應(yīng)用變換關(guān)系P，得到移動(dòng)部件驗(yàn)證點(diǎn)的目標(biāo)位置信息OTest；

(14) 對比MTest與OTest兩組驗(yàn)證點(diǎn)的偏差。計(jì)算偏差平均值

與標(biāo)準(zhǔn)差σTest。

3.2 實(shí)驗(yàn)數(shù)據(jù)

下面根據(jù)3.1節(jié)中的實(shí)驗(yàn)方案進(jìn)行3組實(shí)驗(yàn)。

為實(shí)際固定部件與移動(dòng)部件分別選取4個(gè)測量點(diǎn)，即m=n=4。(實(shí)際應(yīng)用過程中，應(yīng)根據(jù)實(shí)際測量點(diǎn)的選取情況確定m與n的取值。)

固定部件理論模型上4個(gè)對應(yīng)點(diǎn)位信息M如表1所示。

移動(dòng)部件理論模型上4個(gè)對應(yīng)點(diǎn)位信息N如表2所示。

表2 移動(dòng)部件理論模型對應(yīng)點(diǎn)位信息N

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

表3 驗(yàn)證實(shí)驗(yàn)結(jié)果

(待續(xù))

(續(xù)表)

(待續(xù))

(續(xù)表)

3.3 結(jié)果分析

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

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

4 結(jié) 論

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

[1]呂景亮,張春富,唐文彥,郝欣宇.Indoor GPS 工業(yè)測量系統(tǒng)自標(biāo)定技術(shù)研究[J].計(jì)量學(xué)報(bào),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ī)大部件對接中的位姿計(jì)算方法[J].航空制造技術(shù),2011,(3): 91-94.

[4]陳 宇,白征東.基于非線性最小二乘算法的空間坐標(biāo)轉(zhuǎn)換[J].大地測量與地球動(dòng)力學(xué),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ī)裝配中的應(yī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]滕志遠(yuǎn),張愛武.單位四元素法在激光點(diǎn)云坐標(biāo)轉(zhuǎn)換中的應(yīng)用[J].測繪通報(bào),2010(11): 7-10.