面向激光跟蹤測量的大范圍高精度姿態測量

高豆豆，董登峰*，邱啟帆，崔成君，王 博

（1.中國科學院 微電子研究所，北京 100029；2.中國科學院大學，北京 100049；3.南京航空航天大學 自動化學院，江蘇 南京 211106）

1 引言

隨著我國航空航天、船舶潛艇、高鐵汽車和大型科學裝置等高端裝備的快速發展以及先進制造業的日新月異，國內相關廠商對精密測量、精密制造儀器設備的需求越來越迫切。

在航天發動機總裝、航空機翼機身裝配、飛行器安裝、汽車裝配、大型船舶裝配對接和工業機器人校準補償等大型高端智能制造領域［1-2］，裝配制造系統需要同時獲取目標的空間坐標信息與姿態信息，傳統的三自由度空間坐標測量技術已經不能滿足測量要求，因此，亟需發展高性能大范圍的空間姿態測量技術，形成新型空間六自由度測量系統。當前，國內關于空間位置三自由度測量技術日趨成熟，但空間姿態三自由度高性能測量技術仍面臨巨大挑戰。

空間姿態包括方位角、俯仰角和滾動角空間三維轉角。面向姿態測量，Liu F 等［3］提出了一種用于固定翼飛機相對姿態測量的單目非機載視覺系統，在實際飛行實驗中最大姿態測量誤差優于1.0°。薛偉等［4］基于迭代最小二乘法與單目視覺設計了一種氣浮平臺姿態測量方法，動態測量精度約為0.12°。徐洋等［5］設計了一種適用于高超聲速風洞的雙目視覺測量系統，在1.2 m 測量距離內姿態測量誤差優于0.08°。Zeng 等［6］提出了一種利用雙目視覺檢測輔助標記點測量液壓支架姿態的方法，可以適應煤礦生產的惡劣工作環境。Wang 等［7］設計了一種基于單目線掃描視覺的姿態測量系統。Yang 等［8］依托卷積神經網絡設計了一種用于虛擬現實技術的單目視覺位姿估計方法。Li 等［9］基于紅外視覺相機與激光位移傳感器實現六自由度測量，姿態測量精度達到1.1°。Wan 等［10］融合視覺與慣性傳感器實現了對掘進機的位姿測量。陳平等［11］基于視覺與力覺組合測量沉孔零件位姿，角度誤差達到1.8°。

綜上可知，視覺測量具有模型簡單、穩定性強的特點，與基于視覺的多傳感器融合測量是一類通用的姿態測量方法。相較于多傳感器融合方案，單目視覺測量因為涉及的坐標系與誤差源較少，可作為姿態測量的首選方案。經典的視覺姿態測量方法可以分為兩類：以EPnP 算法［12］為代表的通用算法，只能用于解析特征點對應關系已知的情況，不具備泛用性；以SoftPOSIT 算法［13］為代表的同時實現特征點匹配與姿態解算的算法，則需要輸入較為精確的初始位姿信息用于解算，否則無法完成精準解析。在實際工程環境中，特征點對應關系與初始姿態信息大多為未知量，因此，如何實現工程測量中的姿態測量成為一項研究重點。

面向激光跟蹤的姿態測量技術研究起步較早，主要成果有API STS，Leica T-Mac/TScan/T-Probe 等跟蹤控制探測器［14］，使用時需要匹配相應的激光跟蹤儀。國內針對該領域的姿態測量技術研發尚未成熟。天津大學團隊針對盾構導向應用，以全站儀為基站，研究了融合光學、視覺、IMU 等多傳感器的姿態測量技術［15］，但測角量程較小。湖北工業大學與中國科學院微電子研究所合作研究了單目視覺姿態測量方法、PSD 與單目視覺融合測量方法、加權正交迭代法等［1，16-18］，最優測角精度為0.11°，仍有提升空間。周道德等［19］提出基于深度學習與PnP 模型的姿態測量方法，在3 m 內其測試精度達到0.03°，但在激光跟蹤測量場景中解算錯誤的概率較大，實際應用受限。Meng等［20］展望了PSD 在姿態測量中的前景，但未實現實際應用。

為提升激光跟蹤測量系統的姿態測量精度與測量范圍，并解決特征點匹配關系未知時需要初始位姿信息的問題，本文提出一種基于單目視覺的大范圍全自動姿態測量方案，該方案測量精度高，具有廣泛的適用性。

2 姿態測量系統設計

2.1 硬件設計

姿態測量系統通過單目視覺單元與空間合作目標配合實現姿態角的高精度大范圍測量，如圖1 所示。激光跟蹤測量系統主機內的主要模塊包括單目視覺模塊、空間坐標測量模塊與跟蹤模塊，各模塊間為剛性連接。空間合作目標為六自由度合作靶標，表面裝有多個紅外LED，作為識別特征點，中心安裝有角錐棱鏡。

圖1 激光跟蹤測量的姿態測量系統組成Fig.1 Composition of attitude measurement system for laser tracking measurement

單目視覺模塊包括高速采集相機、電動調焦鏡頭、電控單元及數據傳輸單元，用于大范圍自適應清晰捕捉合作目標與姿態解算。空間坐標測量模塊內置激光器，激光跟蹤測量系統工作時，激光器產生穩定出射光，出射光經光路折射至跟蹤模塊，然后經主機出光口入射至合作靶標中心的角錐棱鏡，并折返回主機。依據折返光方向，跟蹤模塊控制調整主機方向，使出射光始終正對角錐棱鏡中心，保持主機對合作靶標的跟蹤狀態。根據折返光與出射光的回程差，空間坐標測量模塊解析出精確的空間測距值，即系統工作距離，結合主機轉動量，進一步解算得到合作靶標的空間坐標。由于系統具有跟蹤性能，且主機內各模塊為剛性連接，因此，合作靶標在任意移動時可始終處于單目視覺模塊視場內，避免合作目標無法捕捉的情形。又因系統的工作距離實時可知，電動調焦鏡頭依據這一參數可自適應調節景深，實現單目視覺模塊對合作靶標的大范圍清晰成像。

基于特征點個數對姿態解算精度的影響［1］，合作靶標特征點數選取10～16 個，非共面分布在合作靶標表面。圖2 展示了一種布局策略，14 個紅外LED 主動發光點均勻分布在合作靶標表面，從上到下為“2-6-6”三層分布。每層特征點個數大于1，所有特征點滿足非共面要求，構成冗余約束，用于降低特征點質心坐標檢測誤差對姿態解算精度的影響。

圖2 合作靶標紅外LED 布局Fig.2 Infrared LEDs' layout of cooperative target

2.2 數學模型

如圖1 所示，姿態測量系統共包含3 個剛體坐標系，分別為激光跟蹤測量系統坐標系OLXLYLZL、視覺模塊坐標系OC-XCYCZC（與相機坐標系重合）和合作靶標坐標系OT-XTYTZT。

記任一點空間坐標為p，則該點在3 個坐標系間的轉換關系可表示為：

由式（1）可得：

為方便精度評價，定義合作靶標繞激光跟蹤測量系統坐標系按照“X-Y-Z”順序旋轉，繞X軸旋轉角為俯仰角α，繞Y軸旋轉角為方位角β，繞Z軸旋轉角為滾動角γ。建立姿態角與旋轉矩陣的轉換關系如下：

3 姿態測量算法

姿態測量算法的核心為解算合作靶標坐標系相對于相機坐標系的轉換矩陣，合作靶標外表面LED 為特征點集，記LED 個數為N。LED 質心在合作靶標坐標系下的坐標為TP={(Xi，Yi，Zi)T}，在相機坐標系下的坐標為CP={(xi，yi，zi)T}，在像素坐標系下的理想坐標為IP={(ui，vi，1)T}，其中i∈[1，N]。轉換關系如下：

其中：(fu，fv，u0，v0)分別為像素坐標系兩個方向的等效焦距與相機中心點像素坐標，s=1為縮放因子。由于鏡頭成像存在畸變，特征點的實際像素坐標與式（4）存在一定差異，需要對圖像進行畸變校正，IP為校正后坐標。

3.1 實時相機成像模型

電動調焦鏡頭依據鏡頭的光學傳遞函數、彌散斑在不同后截距、工作距離時的表現情況，可以確定不同物距下的最優后截距，并進行函數擬合，如圖3 所示，實現2～20 m 測量范圍內合作靶標的清晰成像。圖3 中，dw為工作距離，fb為后截距。

圖3 電動調焦鏡頭工作距離-后截距擬合曲線Fig.3 Working distance-back focal length fitting curve of electric focusing lens

實時調焦過程中隨著后截距的改變，成像畸變信息會發生較大的變化，而圖像中心點位置幾乎無變化。因此，(fu，fv，u0，v0)隨后截距變化產生的變化量非常微小，可忽略不計，采用張正友標定法即可完成數值標定。由于圖像畸變信息動態變化，采用傳統標定方法標定畸變參數則需要采集大量的數據，耗時耗力，為實現快速標定，本文基于光學仿真數據建立了畸變信息數學模型，實現快速實時圖像校正。

根據鏡頭光學模型的多參數畸變仿真，得到單目視覺模塊畸變率隨工作距離、光斑投影位置變換而產生的變化，如圖4 所示，光斑為合作靶標的LED 經電動調焦鏡頭投影到相機上的像。其中，工作距離dw為0～20 m，光斑質心位置dp為0～5.5 mm，表示為：

圖4 單目視覺模塊成像畸變的數學模型Fig.4 Mathematical model of monocular vision module imaging distortion

其中：dp為光斑質心與相機中心點間的距離，c為相機的像元尺寸；(ut，vt)為光斑質心像素坐標，經數字圖像處理技術解析獲取。

畸變率表述為：

其中：Rdis為畸變率，dpI為理想光斑質心與相機中心點間的距離。

由圖4 可知，圖像畸變率與光斑位置、工作距離存在復雜的變化關系，無法建立單一數學模型。為精確校正畸變，依照圖像畸變率的全局變化趨勢與函數擬合置信度統計，本文采用分段式建模。對工作距離進行4 組劃分，分別為（0，3.7），［3.7，4.4），［4.4，9），［9，20］，單位為m，分別進行多項式擬合。結合式（5）、式（6）與多項式擬合規律，得到理想光斑質心與實際光斑質心、工作距離的關系如下：

dw具有 4 個區間，因此式（7）中的pij(i，j∈[0，3])共有4 組。

由于理想光斑質心位置位于實際光斑質心與相機中心點的連線上，因此二者存在相似的三角形比例關系，IP內各像素點坐標記為：

代入式（4）解得CP，完成相機坐標系下特征點坐標的求解。

3.2 位姿解算

在解算特征點物點和像點對應關系未知情況下的高精度姿態時，通常需要操作人員根據觀測圖像進行手動匹配［16］或者構建大量初始位姿信息庫并通過枚舉法進行解算［21］。前者不適用于全自動化測量，當姿態角測量范圍大時，后者初始數據庫信息量會爆炸式增長，無法滿足對實時性有要求的場合。因此，為實現全自動化姿態測量，本文對EPnP 算法和SoftPOSIT 算法相結合的方法［16］進行了改進，建立初始特征點自動匹配算法，改進特征點匹配的權重分配策略，實現姿態信息自動初始化與結果監測，完成大范圍實時自動高精度姿態測量。

3.2.1 初始信息解算

PT與PI的匹配關系是未知的，但由圖2 可知，合作靶標上最外圍LED 編號1，3，12，14 可構成一個四邊形，且在合作靶標轉動過程中圖像上4 點的相對位置關系變化較小，因此可將其作為定位點，基于幾何關系完成特征點匹配，解算應用于SoftPOSIT 算法的初始位姿信息。

記PI中各點的歐氏距離矩陣為Mdist：

取Mdist中的最大值與次大值可定位兩組邊緣的對角線點，利用四邊形頂點間的幾何關系即可確定4 點的匹配關系。將其像素坐標與合作靶標坐標系坐標按照匹配關系帶入EPnP 算法，得到初始位姿信息。其中，LED 在合作靶標坐標系下的坐標由合作靶標設計提供。

這4 點位于同一平面，得到的位姿信息必然是不夠精確的，因此不能直接用于精密測量。該方法不局限于圖2 一種布局方案，適用于所有最外圍特征點可構成一個歐氏距離相對關系唯一確定的布局方案。

3.2.2 高精度姿態解算

由EPnP 算法解算的初始位姿信息可以確保SoftPOSIT 算法正確收斂，實現高精度測量。SoftPOSIT 算法包含兩層算法：softAssign 算法，進行特征點匹配優化；POSIT 算法，解算位姿信息。求解過程即兩層算法交替迭代尋找最優匹配方案使目標函數最小：

其中：N為特征點個數，M為檢測到的像素特征點個數，mij（mij∈[0，1]）為特征點匹配概率矩陣MP（MP∈RN×M）的元素，(Q1，Q2，Pi，wi)為位姿推導過程量，具體含義可參考文獻［13］，(xj，yj)為像素特征點的歸一化坐標，α為匹配閾值。

為提升正確匹配的特征點在姿態解算中的權重，在SoftPOSIT 算法匹配結束后，對mij進行再優化。分以下兩種情況討論：

當mij所在行和列的其他元素值均為0 時，認為mij所對應的特征點匹配正確，將mij賦值為1。

當mij所在行和列的其他元素值存在非0 值時，參照Leaky RuLU 函數［22］對mij所在行和列的元素進行如下變換：

其中：α為概率閾值，γ為懲罰因子。依照實驗分析，α=0.55，γ=0.5。

3.2.3 算法流程

由于工業檢測現場環境復雜，可能存在使SoftPOSIT 算法陷入局部最優解的情況，且當解算結果出現異常值時該算法并不具備自動校正功能，因此，EPnP 算法與SoftPOSIT 算法簡單的串行應用并不能實現高精度自動姿態測量。本文針對這一缺陷，提出了一種自動監測糾錯機制，實現全自動姿態測量。

如圖5 所示，電動調焦鏡頭依據測距信息實時調整狀態，保證單目視覺模塊能夠實時清晰成像。待電動調焦鏡頭調整穩定后，相機捕獲圖像，進行姿態解算。由SoftPOSIT 算法精確解算位姿信息后，系統將解算的坐標信息與測距信息進行對比，若差值大于異常值閾值，則觸發初始信息校正；若小于，則輸出解算信息。

圖5 姿態測量算法流程Fig.5 Flow chart of attitude measurement algorithm

4 測量實驗與結果

為驗證方案的可行性與測量精度，構建實驗裝置如圖6 所示。單目視覺測量模塊與空間坐標測量模塊集成在儀器主機內部，合作靶標安裝于姿態測量轉臺上，轉臺為角度發生裝置。單目視覺測量模塊內，相機為HIKROBOT MV-CH050-10CM，分辨率為2 432×2 048，電動調焦鏡頭焦距為50 mm。本系統在精度測試前已完成相機坐標系與激光跟蹤測量系統坐標系的標定，因此，本實驗的精度評價數據均為激光跟蹤測量系統坐標系下的測量數據。測試用轉臺為精密二維轉臺，該轉臺的最大角定位誤差為7.9″，重復定位精度為1.8″。

圖6 姿態測量精度測試實驗裝置Fig.6 Attitude measurement accuracy experimental equipment

為驗證方案的通用性，共搭配兩種規格的合作靶標進行實驗。合作靶標1 安裝有14 個紅外LED，LED 布局如圖2 所示。合作靶標2 安裝有10 個紅外LED，LED 布局為“2-4-4”。與合作靶標1 相比，合作靶標2 減小了體積與LED個數。

轉臺與合作靶標間使用機械件轉接，依靠機械加工保證二者為剛性連接。在精度測量開始前，開啟激光跟蹤測量系統跟蹤功能，令主機出射光始終跟隨合作靶標。將合作靶標安裝到轉臺上，之后開啟激光跟蹤測量系統姿態測量功能，上位機自動接收測量結果，參照姿態角解析結果調整轉臺轉角直至激光跟蹤測量系統三維姿態角測量值均近似為0°，此時三者坐標系近似平行。該轉臺為二維轉臺，具有俯仰軸與滾動軸，因此在進行方位角測量時，調整轉臺滾動軸旋轉90°，使轉臺俯仰軸與合作靶標方位軸近似平行，此時，轉臺俯仰軸的轉動量等效為激光跟蹤測量系統測量的方位軸轉動量。由于轉臺與合作靶標的坐標系無法做到完全平行，為減小這一因素引入的精度誤差，本實驗的單步姿態角測量精度評價公式為：

其中：Δαi為第i次測量的俯仰角誤差，αi為第i次激光跟蹤測量系統測量的俯仰角，θi為第i次轉臺轉動俯仰角，同理β表示方位角，γ表示滾動角，i，j，k=1，2，3，…。

同一距離下姿態角測量精度的評價公式為：

其中：Δα，Δβ，Δγ分別為俯仰角、方位角和滾動角的最大絕對誤差。

為方便數據比較，本實驗分別在儀器主機與合作靶標距離3，10 m 處進行了實驗。測量精度的評價范圍如下：俯仰角、方位角測量為±30°，測角步長為5°；滾動角測量范圍為±180°，測角步長為30°。將各坐標系調整至近似平行后，將轉臺轉角清零，記錄此刻轉臺的角度值與激光跟蹤測量系統測量值為各自零位。而后按照測角步長轉動轉臺，待轉臺轉角穩定后，記錄此刻轉臺角度值與激光跟蹤測量系統測量值為該步長對應的數值。

4.1 合作靶標1 測試結果

圖7 詳細展示了3，10 m 處合作靶標1 的單步姿態角測量誤差。其中，每次測量的起始點為激光跟蹤測量系統測量值的零位。表1 匯總了合作靶標1 的測量精度，10 m 內可實現三維姿態角的連續自動測量，姿態測量精度優于0.049°。

表1 合作靶標1 姿態角測量結果Tab.1 Result of attitude measurement with cooperative target 1

圖7 合作靶標1 姿態角測量誤差Fig.7 Attitude measurement error with cooperative target 1

4.2 合作靶標2 測試結果

圖8 詳細展示了3，10 m 處合作靶標2 的單步姿態角測量誤差。表2 匯總了合作靶標2 的測量精度，10 m 內姿態角測量精度優于0.065°。

表2 合作靶標2 姿態角測量結果Tab.2 Result of attitude measurement with cooperative target 2

圖8 合作靶標2 姿態角測量誤差Fig.8 Attitude measurement error with cooperative target 2

綜合合作靶標1、合作靶標2 的測量數據可以發現，對于同一合作靶標，使用定焦鏡頭作為成像元件，隨著工作距離的增加，激光跟蹤測量系統的姿態角測量精度會逐漸下降；對于不同合作靶標，使用同一套測量硬件與測量方法，隨著特征點個數的增加，激光跟蹤測量系統的姿態角測量精度會有所提升。

表3 對比了近幾年面向激光跟蹤測量的姿態測量方法的測量范圍與測量精度。其中，視覺+激光準直［1］采用最大絕對誤差值作為評價指標，測量距離為2.5 m，測量角度是（±30°，±30°，±180°）；POSIT［2］與WAOI［2］采用均方根誤差作為評價指標，測量距離為15 m，測量角度是（±20°，±20°）；加權融合法［18］采用最大絕對誤差值作為評價指標，測量距離為8 m，測量方位角是±30°。相較于上述方法，本文實現了10 m 內三維姿態角的全自動測量，角度覆蓋（±30°，±30°，±180°），匹配兩種不同的合作靶標，測量精度分別可達到0.049°，0.065°，具有明顯的優勢。由圖7 和圖8 可知，測量數據包含一定的系統誤差，可能是由于轉臺與合作靶標間的坐標系不完全平行引入的，尚無較優的解決方案，因此本文的實際測量精度應優于0.065°。

表3 面向激光跟蹤測量的姿態測量方法精度對比Tab.3 Comparison of attitude measurement accuracy for laser tracking measurement

測量過程中，合作靶標可以安裝在被測物表面或者手持自由移動，視覺模塊可自適應實時清晰成像，完成動態姿態測量，單次姿態測量耗時為60～90 ms。依靠改進的EPnP 算法與Soft-POSIT 算法的結合方案，合作靶標可以在任意位置自動開啟姿態測量功能與自動糾錯功能，若合作靶標脫靶并重新連接，也可以自動恢復姿態測量功能。

5 結論

本文面向精密激光跟蹤測量的精度需求，提出一種基于自適應視覺單元的姿態測量方法，實現了激光跟蹤測量系統的大范圍全自動姿態測量。設計了激光跟蹤測量的姿態測量系統，建立了數學模型，明確三維姿態測量的定義。結合光學成像特性與張正友標定法，建立了實時相機成像模型、動態校正特征點質心像素坐標提取；約束合作靶標上特征點布局的幾何特性，基于EPnP 算法與SoftPOSIT 算法優化姿態解算流程，實現全自動姿態測量。實驗數據表明，在方位角/俯仰角為±30°、滾動角為±180°，測量距離10 m 內，配合安裝14 個LED 的合作靶標，三維姿態角的測量精度優于0.049°；配合安裝10 個LED 的合作靶標，三維姿態角的測量精度優于0.065°。相較于已有方法，本方法在測量范圍與測量精度上均具有明顯的測量優勢。由于定焦鏡頭成像的局限性，要保證姿態測量精度不隨工作距離的增加而降低，可將成像鏡頭替換為高分辨率變焦鏡頭，從而保證合作靶標的圖像在工作距離內等倍變化。