張文哲 項斌斌 王娜 王兆軍 古麗加依娜哈再孜汗，3

(1 新疆大學物理科學與技術學院 烏魯木齊 830046)

(2 中國科學院新疆天文臺 烏魯木齊 830011)

(3 中國科學院大學 北京 100049)

1 引言

在天文領域， 射電望遠鏡通常采用六自由度(6-DOF)并聯(lián)機構[1-2]調(diào)整天線副面位姿， 為了能夠提高天線副面并聯(lián)調(diào)整機構的控制精度， 需要精準測量六自由度并聯(lián)機構動平臺的位姿. 目前， 對六自由度并聯(lián)機構主要是通過傳感器[3]或激光跟蹤儀[4]來檢測其動平臺位姿， 而單目視覺測量系統(tǒng)僅需使用一臺相機解算單幅圖像的位姿信息， 與其他視覺測量系統(tǒng)相比， 具有結構簡單、測量范圍大、相機標定簡單等優(yōu)點， 避免了立體匹配的困難， 廣泛應用于自動導航、機器人跟蹤、飛行器對接等多個領域[5].

六自由度并聯(lián)機構動平臺的相對位姿解算是單目視覺位姿測量系統(tǒng)的核心， 解算的準確度越高， 相對位姿測量越精確， 對此研究者們提出了很多位姿解算方法. 其中最常用的單目視覺位姿解算方法是基于特征點的位姿解算方法， 即n點透視(Perspective-n-Point， PnP)問題， 其中n為特征點的個數(shù)[6]. Fischler和Bolles于1981年首次提出PnP問題[7]. Quan等人提出了一種針對4個點和5個點的情形， 構成一個P3P問題利用Singular Value Decomposition (SVD)分解的解算算法， 拓展到n個點的線性解算方法[8]. SVD算法容易受誤差影響， 當誤差較大時算法精度會迅速下降. 霍炬等人以平面P4P幾何為約束條件， 采用改進的兩段式迭代算法解算位姿， 并給出了特征標靶最優(yōu)布局， 以獲得更高的測量精度[9]. 張世杰等人利用目標航天器本身的特征信息， 采用P3P問題的解析方法獲得迭代初值， 再采用Levenberg-Marqurdt(LM)算法最終迭代出目標的相對位姿[10]. 這種方法的最終解算精度與初值選取有關， 需確保初值選取的準確性， 否則算法可能會不收斂或收斂到錯誤的位姿導致無法使用. Lepetit等人于2009年提出了非迭代高精度快速位姿估計算法-EPnP (Efficient Perspective-n-Point)算法[11]， 該算法具有時間復雜度低、計算量相對較小等特點， 被認為是目前最高效的位姿估計算法之一. 武君等人為了分析POSIT (Pose from Orthography and Scaling With Iteration)與EPnP兩種算法的優(yōu)缺點， 通過大量實驗， 從特征點數(shù)、噪聲、實時性等方面進行對比，結果顯示EPnP算法的測量精度高，POSIT算法的求解速度快[12]. 分析得出， 對于PnP問題， 算法的解算精度與特征點坐標提取精度有關， 坐標提取精度越高， 位姿解算精度越高.

為了快速精準測量六自由度并聯(lián)機構動平臺的位姿， 本文提出了一種基于非共面合作目標和EPnP算法結合的單目視覺位姿測量方法. 文章內(nèi)容安排如下: 首先介紹了并聯(lián)機構位姿的測量方案， 然后對EPnP算法和平方加權質心法進行了描述， 通過特征點在三維空間坐標系和二維圖像坐標系之間的對應關系和空間位置約束， 解算出并聯(lián)機構的相對位姿， 最后通過單目視覺位姿測量實驗驗證了本方法的精度和有效性.

2 六自由度并聯(lián)機構相對位姿測量方案

本文采用單目視覺技術對六自由度并聯(lián)機構動平臺位姿進行實時測量， 測量系統(tǒng)如圖1所示， 合作目標固定于動平臺上， 相機固定于靜平臺上， 動平臺相對于靜平臺的運動等效于合作目標相對于相機的運動. 當動平臺運動時， 相機實時拍攝動平臺上合作目標的圖像， 然后采用單目視覺技術得到動平臺的位姿. 該測量系統(tǒng)的工作原理是控制安裝于相機頂部的照明單元對合作目標進行照明， 合作目標上各高反標志點將入射光準原路反射回去之后進入相機視場成像， 對圖像進行特征處理后， 提取出有效的合作目標信息， 再采用位姿解算算法獲得動平臺相對位姿信息.

圖1 單目視覺測量系統(tǒng)示意圖Fig.1 Schematic diagram of the monocular vision measurement system

并聯(lián)機構相對位姿測量流程如圖2所示， 主要測量過程如下:

圖2 系統(tǒng)流程圖Fig.2 System flow chart

(1)圖像采集和相機標定

控制照明單元對合作目標進行照明， 采集合作目標運動圖像， 采用張正友標定法[13]獲得相機內(nèi)部參數(shù).

(2)圖像預處理和圖像特征提取

對圖像進行去噪處理、灰度二值化、閾值分割等操作， 再采用平方加權質心法[14]提取特征點圖像中心坐標.

(3)單目視覺位姿解算

采取EPnP算法對特征點圖像中心坐標進行位姿解算， 通過與多個非共面特征點結合的方法降低圖像噪聲的影響并提高算法的抗干擾能力. 該算法的核心思想是利用三維線性空間的一組基(4個非共面虛擬控制點)線性加權表示空間三維特征點，把求解n個特征點在相機坐標系下的坐標轉換為求解4個虛擬控制點在相機坐標系下坐標的問題， 從而降低了算法的復雜度[15]， 然后通過Horn等人提出的絕對定位算法[16]求解合作目標相對于相機的旋轉矩陣R和平移向量T.

3 單目視覺位姿測量原理

3.1 單目視覺位姿測量模型

本文所采用的單目視覺位姿測量方法是用單個相機拍攝合作目標圖像， 經(jīng)過上述位姿測量流程， 最終解算出目標坐標系相對于相機坐標系的姿態(tài)和位置信息. 采用的相機成像模型是針孔成像模型[17]， 該成像模型需要建立4個坐標系， 分別是目標坐標系、相機坐標系、圖像物理坐標系以及圖像像素坐標系， 坐標系之間的關系如圖3所示. 以合作目標上某一特征點為原點， 建立目標坐標系Ow-XwYwZw， 跟隨合作目標一起運動; 以相機的光心為原點， 以光軸為Zc軸， 建立相機坐標系Oc-XcYcZc; 圖像坐標系所在平面為相機的成像平面， 建立圖像物理坐標系O-xy和圖像像素坐標系O0-uv. 圖中Oc、f、Zc分別為相機的光心、焦距、光軸，C1、C2、C3、C4代表虛擬控制點， 以黑點表示特征點，P為其中之一.

圖3 單目視覺位姿測量原理Fig.3 The principle of monocular vision pose measurement

設點P在相機坐標系下的坐標為(Xc，Yc，Zc)，在目標坐標系下的坐標為(Xw，Yw，Zw)， 在圖像像素坐標系下的圖像坐標為(ui，vi)， 經(jīng)過標定獲得相機的內(nèi)參矩陣為A， 圖像主點坐標為O0= (u0，v0)，則P點在彼此坐標系下的關系為:

3.2 位姿解算算法

EPnP算法中4個虛擬控制點一般是人為選取， 通常情況下，C1為特征點集的質心，C2、C3、C4為特征點集主分量分解后的3個分量.

通過(7)式和(8)式求出ξ值和X值， 即可得到4個虛擬控制點在相機坐標系下的坐標. 通過(3)式得到特征點在相機坐標系下的坐標， 從而將求解三維到二維的PnP問題變換為求解經(jīng)典的三維到三維的剛體運動問題， 最后通過Horn絕對定位算法得到合作目標相對于相機的姿態(tài)角和平移量.

3.3 圖像特征點中心坐標的提取

由上述位姿解算算法可知， PnP問題的求解主要依靠標定過的特征點的空間相對位置和圖像特征點的坐標， 所以位姿的測量精度直接取決于圖像特征點的中心定位精度.

為了提高特征點的定位精度， 合作目標選用非共面高反標志點和濾光片， 相機鏡頭也安裝了相同的濾光片. 為了利用特征點z方向上的信息選擇非共面的合作目標特征點， 可以更好地檢測出合作目標的微小轉動. 測量過程中通過光源照射合作目標， 得到反射光斑， 獲得高對比度的準理想圖像， 從而降低光源的影響， 并簡單有效地劃分了圖像特征區(qū)域. 本文選用平方加權質心法對圖像特征點中心坐標進行提取. 設f(x，y)為二維灰度圖像中位于坐標位置(x，y)的像素點的灰度值，m代表像素點，0＜x ＜mj， 0＜y ＜mj， 則其數(shù)學描述如下:

平方加權質心法采用灰度值的平方作為權值， 加強了距離中心較近的像素點對質心的影響， 減少了噪聲帶來的誤差. 最后將提取出的中心坐標代入位姿解算算法中， 得到合作目標與相機之間的姿態(tài)和位置信息.

4 實驗和結果分析

4.1 實驗描述

為了檢驗位姿測量方法的精度， 搭建了六自由度并聯(lián)機構單目視覺位姿測量實驗平臺， 六自由度并聯(lián)機構如圖4所示， 實驗平臺如圖5所示， 主要由相機探測單元和合作目標組成. 相機探測單元中相機采用一體化設計， 光學鏡頭、成像探頭、數(shù)據(jù)處理電路、照明電路等組件均在同一機箱內(nèi); 相機的分辨率為2592× 2048， 鏡頭焦距16 mm. 合作目標由7個圓形高反射率標志和濾光片組成， 以一定構型安裝在支架上， 其中6個標志點位于底面直徑為140 mm的同一圓周上， 第7個特征點位于正中央，相對其他6個點有60 mm的高度差. 相機和合作目標如圖6所示; 實驗中相機拍攝合作目標， 處理過的圖像如圖7所示.

圖4 六自由度并聯(lián)機構Fig.4 6-DOF parallel mechanism

圖5 實驗平臺Fig.5 Experimental platform

圖6 相機和合作目標Fig.6 Camera and cooperation target

圖7 特征點圖像處理圖Fig.7 Image processing chart of feature points

相對位姿的測量包括目標坐標系沿相機坐標系的x軸、y軸、z軸的平移距離和旋轉角度， 其中繞x軸旋轉得到α， 繞y軸旋轉得到β， 繞z軸旋轉得到γ.

4.2 結果分析

通過改變六自由度并聯(lián)機構動平臺的平移距離和旋轉角度， 將合作目標相對于相機的位置和姿態(tài)參數(shù)作為測量值， 然后拍攝圖像測試方法的有效性. 以垂直于相機光軸位置為零位， 選擇的平移范圍為±30 mm，依次沿x、y、z軸平移5 mm;旋轉范圍為±5°， 依次沿x、y、z軸旋轉1°. 將測量值與理論值進行比較， 得到平移距離和旋轉角度的位姿測量誤差曲線， 結果如圖8所示; 對圖8進行數(shù)據(jù)分析，分別得到平移距離誤差表和旋轉角度誤差表， 如表1和表2所示. 均方根誤差(RMSE)表示的是測量值與真實值之間的偏差， 代表著測量值相對于平均值的離散程度， RMSE越小代表著數(shù)據(jù)越聚集， 通常情況下， RMSE越小測量精度越高.

由表1和表2數(shù)據(jù)可得， 各方向上的最大誤差和RMSE分別為:x方向0.0440 mm和0.0232 mm;y方向0.0838 mm和0.0325 mm;z方向0.0938 mm和0.0672 mm;α方向0.0333°和0.0146°;β方向0.0280°和0.0159°;γ方向0.0221°和0.0090°. 由圖8可知， 當動平臺偏離靜平臺時， 沿x軸和y軸的平移和旋轉方向上的測量誤差呈現(xiàn)相對增大趨勢， 主要原因是合作目標逐漸偏離相機的測量范圍， 降低了圖像點的提取精度， 從而使圖像處理誤差增大; 沿z軸平移方向上的測量誤差從零位開始逐漸增大， 相對于其他方向的誤差，z方向較大， 主要原因是動平臺的抖動和景深增大， 前者在動平臺沿z軸上下移動時導致合作目標偏離原本固定于動平臺的位置， 使得解算出來的位姿出現(xiàn)誤差， 而后者降低了圖像點的提取精度; 但沿z軸的旋轉誤差相對較小， RMSE也較小， 主要由于水平轉動不影響圖像處理精度. 由實驗數(shù)據(jù)可知， 相機與合作目標間平移±30 mm范圍內(nèi)， 測量誤差小于0.1 mm. 旋轉±5°范圍內(nèi)， 測量誤差小于0.05°， 該測量精度滿足六自由度并聯(lián)機構的精度要求.

圖8 位姿測量誤差: (a) x軸平移誤差; (b) y軸平移誤差; (c) z軸平移誤差; (d) α旋轉角誤差; (e) β旋轉角誤差; (f) γ旋轉角誤差.Fig.8 Pose measurement error: (a) translation error of x-axis; (b) translation error of y-axis; (c) translation error of z-axis; (d)the rotation angle error in α direction; (e) the rotation angle error in β direction; (f) the rotation angle error in γ direction.

表1 平移距離測量誤差Table 1 Translational distance measurement error

表2 旋轉角度測量誤差Table 2 Rotation angle measurement error

實驗中除了因為環(huán)境、設備、圖像處理和算法解算等因素引起的測量誤差外， 合作目標的制作精度也可能會帶來誤差， 制作時合作目標上特征點之間的真實距離與給定的理論距離的差值會帶來特征點坐標的誤差. 因此， 想要進一步提高位姿測量精度， 需要對測量誤差的來源進行分析， 如可采用精度更高的相機及合作目標提高圖像處理精度，保證合作目標緊緊地固定在動平臺上， 或通過增加特征點的個數(shù)提高測量精度， 特征點越多測量精度越高. 一般需要根據(jù)被測物體的結構尺寸、實時性、精度等要求選擇適當數(shù)量的特征點， 因為太多會增大圖像處理和位姿解算算法的運算時間和復雜度， 而特征點太少的話， 當在相機拍攝合作目標運動圖像過程中， 合作目標上的特征點可能會發(fā)生遮擋或丟失等情況導致提取的特征點個數(shù)不足， 影響測量精度.

5 結論

為實時檢測六自由度并聯(lián)機構動平臺的位姿，本文提出了一種基于非共面合作目標與EPnP算法結合的單目視覺位姿測量方法， 該方法通過獲得的高對比度圖像和提取出的高精度中心坐標，再采用EPnP算法對動平臺的位姿進行求解. 通過樣機研制與實驗測試可知， 并聯(lián)機構動平臺在平移±30 mm范圍內(nèi)， 沿x、y、z軸方向的RMSE分別為0.0232 mm、0.0325 mm和0.0672 mm， 測量誤差保持在0.1 mm以內(nèi); 在±5°旋轉范圍， 沿x、y、z軸方向的均方根誤差分別為0.0146°、0.0159°和0.0090°， 測量誤差保持在0.05°以內(nèi). 結果表明， 所研制的測量系統(tǒng)精度滿足天線副面并聯(lián)調(diào)整機構的精度需求. 實驗結果驗證了本文所提方法的可行性和有效性， 為實現(xiàn)并聯(lián)機構閉環(huán)控制提供了有效手段.