劉傳凱，雷俊雄，劉茜，孫軍，蘇建華，胡曉東，李東升

1.江西理工大學 電氣工程與自動化學院，贛州 341000

2.北京航天飛行控制中心，北京 100190

3.航天飛行動力學技術重點實驗室，北京 100190

4.中國科學院 自動化研究所，北京 100194

在工業(yè)與航天領域，靶標作為一種輔助工具，常用于工件或目標位姿的精確測量，目前使用最為廣泛的靶標類型有2 種，圓形類靶標與棋盤格類靶標。因圓形目標自身具有平移、旋轉和縮放不變性，易于識別與定位，所以常選用圓形作為靶標模板，在圓上添加幾何要素來構成圓形靶標；棋盤格靶標一般是由大小相等交錯排列的黑白兩色矩形組成，具有中心對稱、角點特征易于獲取等優(yōu)點，所以棋盤格靶標在視覺測量領域也經(jīng)常作為一種輔助測量手段。為實現(xiàn)工件或目標的精確位姿獲取，首先對靶標在圖像中的位置進行精準識別與計算，然后采用PNP（Perspective-N-Point）或 者 EPNP（Efficient Perspective-N-Point）等測量算法實現(xiàn)目標位姿求解，然而受不確定環(huán)境和復雜光照影響，靶標點提取的準確性一直是難以完全解決的問題，也是提升目標定位精度的關鍵。

與棋盤格靶標通過檢測梯度極值點來提取相比，圓形靶標通過檢測和篩選橢圓輪廓來提取的方式具有更好的穩(wěn)定性和準確度。圓形靶標提取根據(jù)基本處理單元的不同，主要采用基于Hough 變換的橢圓檢測與基于弧段的橢圓檢測2 類算法。Hough 變換法使用較為廣泛［1］，其實現(xiàn)思想是通過邊緣提取得到像素點并通過聚類算法來實現(xiàn)圓形目標的檢測，但受限于計算復雜度隨橢圓參數(shù)維度指數(shù)增長的性質，對存儲空間和計算時間耗費較大［2］，因此有學者通過橢圓自身幾何特性［3-4］、多對一映射［5-6］等方式來降低Hough 變換的參數(shù)空間維度從而在一定程度上改善算法性能?；诨《蔚臋E圓檢測算法是由Kim 等［7］提出，通過對多個像素連接的短直線組合特征的檢測來確定弧段特征，進一步將多個弧段特征組合來檢測橢圓。與Hough變換相比，基于弧段的橢圓檢測算法無需大量的內(nèi)存空間、計算效率較高，但由于弧段特性與多弧段組合為同一橢圓的約束檢驗不多，容易出現(xiàn)不同弧段錯誤組合形成假橢圓的情況。近年 來，Lu［8］和Meng［9］等 通 過 改 變 弧 段 提 取 方法、構造鄰接弧段組合矩陣等手段提升橢圓檢測的準確率，但也使得弧段匹配算法復雜，降低了算法效率。

為克服橢圓檢測對靶標定位的影響，期望構建基于特征的空間映射關系來測量目標位姿，減少算法復雜度、提高計算效率的同時保持測量精度。目前較為典型的位姿測量方法是PNP 及其拓展改進方法。其基本思想是通過構建圖像中靶標點與實際空間中靶標真實尺寸的對應關系，實現(xiàn)對目標位姿的計算。根據(jù)使用特征點數(shù)的不同可以將PNP 算法分為2 種情況［10］：一是在特征點數(shù)較少時，使用固定特征點數(shù)的特定求解方法；二是在特征點數(shù)較多時，使用任意數(shù)量特征點進行求解的通用求解方法。對特定求解方法的研究主要集中于P3P（Perspective-3-Point）、P4P（Perspective-4-Point）問題上［11-12］，此類方法實現(xiàn)原理簡單且能得到閉式解，但因使用到的特征點信息較少，對噪聲極其敏感，求解的相機位姿精度不高；通用求解方法利用靶標的冗余信息來提升位姿估計的精度和魯棒性，但計算所需時間也會隨著使用特征點數(shù)目的增加而增加。使用較為廣泛的通用求解方法有直接線性變換法（Direct Linear Transformation，DLT）［13］、正交迭代法（Orthogonal Iteration，OI）［14］、 EPNP 算 法［15］和 直接最小 二乘法［16］等，在處理冗余點數(shù)較多情況取得良好的效果。然而在處理合作靶標冗余量較少的情況時［17］，也難以避免背景噪聲與干擾點仍能對位姿解算精度的影響。

針對上述問題，本文提出了基于逆投影變換的圓形合作靶標相對位姿測量算法，在提取圓形靶標輪廓離散點的基礎上，無需進行橢圓檢測與擬合，將圓形靶標輪廓離散點逆向投影到三維空間中形成過光心和橢圓輪廓點的多個空間射線簇，直接在三維空間中利用射線簇構建描述靶標位姿誤差的殘差函數(shù)，建立對多圓形靶標位姿的整體約束與聯(lián)合優(yōu)化求解模型，從而實現(xiàn)對空間目標相對位姿的低復雜度快速求解，如圖1 所示。第一，該方法避免因橢圓擬合和復雜投影距離計算導致的過程誤差，提高位姿計算效率和求解精度；第二，相較傳統(tǒng)的基于透視投影成像的多特征聯(lián)合像素殘差計算模型，復雜度得到極大簡化；最后，通過在空間中對多個圓形靶標輪廓點集的聯(lián)合平差，提升算法對投影誤差和錯誤點的抗擾能力，使得定位性能明顯改善。

1 圓形靶標逆投影位姿測量

針對利用少量圓形靶標測量空間目標位姿中存在的計算過程復雜、易受噪聲干擾等問題，本節(jié)設計了基于成像光束逆投影的多圓形靶標聯(lián)合平差測量框架，首先利用圓形靶標輪廓點的逆投影方程構建空間射線簇，然后在空間中構建距離殘差函數(shù)和目標位姿優(yōu)化模型，分析模型收斂初始域并設計迭代優(yōu)化算法實現(xiàn)位姿優(yōu)化求解，具體過程詳述如下。

1. 1 空間圓成像投影模型

假設世界坐標系原點為Ow，靶標坐標系原點Ot，相機坐標系原點Oc位于相機光心，靶標坐標系及相機坐標系相對于世界坐標系的變換矩陣wΤt和wΤc分別表示為

式中：wRt∈R3×3和wRc∈R3×3分別表示圓形靶標和相機的姿態(tài)變換矩陣；wXt∈R3×1和wXc∈R3×1分別表示靶標和相機的位置向量。

對于多圓形靶標上第i 個圓形目標輪廓上第j 個空間點P(i，j)，其在世界坐標系和相機坐標系下的位置向量分別表示為wX(i，j)∈R3×1和cX(i，j)∈R3×1。則 空 間 點P(i，j)通 過 透 視 投 影 成 像原理投影到相機圖像中對應的像素坐標u(i，j)為

式中：δM ∈R3×3和pM(i，j)∈R3×3分別表示相機的畸變參數(shù)矩陣和空間點P(i，j)到 相 機 圖 像 的 透 視投影變換矩陣。

令Δu=[Δu，Δv，0]表示相機鏡頭畸變造成的投影點偏移；fu和fv表示相機焦距，單位為像素；u0=[u0，v0，0]T表示圖像主點坐標。畸變修正后空間圓的投影像素坐標為

式中：diag( fu，fv，1)表示對角矩陣；cz(i，j)表示空間點在相機坐標系下的Z 坐標，即cz(i，j)=；n 表示法向量，有n=[0，0，1]T。

1. 2 空間圓輪廓點位姿表示形式

多圓形靶標上第i 個圓形目標輪廓上第j 個空間點P(i，j)的逆投影成像如圖2 所示。其中，wXti表示第i個圓形目標圓心的世界坐標，有wXti=坐標系原點Ot的相對位置關系；wnt表示靶標法向量在世界坐標系下的表示，有wnt=wRtn；wTttXti，式中tXti表示第i個圓形目標圓心與靶標wv(i，j)表示相機指向空間點P(i，j)的單位方向向量，且方向向量穿過空間點P(i，j)在相機圖像上經(jīng)修正后的投影像素坐標，有

圖2 空間圓輪廓點的逆投影成像示意圖Fig.2 Diagram of inverse projection imaging of circular contour points

式中：cv(i，j)表示相機指向空間點P(i，j)的 單 位 方 向向量在相機坐標系的表示。

圓形目標平面任意向量與靶標坐標系法向量wnt垂直，則第i 個圓形目標圓心wXti到空間點P(i，j)的 向 量(wXc+λ(i，j)wv(i，j)-wXti)與wnt垂直，根據(jù)正交向量的內(nèi)積為零，即，得到直線距離λ(i，j)為

因此，空間點P(i，j)的位姿可通過空間機械臂末端相機坐標系表示為

1. 3 位姿優(yōu)化模型的構建及收斂性

設多圓形靶標第i 個圓形目標的半徑為ri，利用目標圓心到空間圓形目標輪廓上任一空間點P(i，j)的距離等于半徑長度，構建空間點P(i，j)的 非線性殘差函數(shù)為

與前期工作［18］不同，多圓形靶標的殘差函數(shù)利用多目標在二維圖像平面與三維空間中的信息計算尺度一致的空間距離作為因變量，充分利用靶標的幾何要素并據(jù)此提升殘差函數(shù)的穩(wěn)定性。

為分析殘差函數(shù)在一定范圍內(nèi)的收斂性質，通過MATLAB 平臺構建多圓離散輪廓點與相機，模擬相機位姿Ω=[x，y，z，θx，θy，θz]T分別發(fā)生(-1 m，1 m)的位置偏移與(-10°，10°)的姿態(tài)偏移時對誤差函數(shù)e(i，j)造成 的影響，如圖3 所 示，其中θx、θy、θz為相機的三個姿態(tài)角。

圖3 誤差函數(shù)隨相機位姿誤差的變化Fig.3 Surface of error function with changes of camera position and pose

從圖3 中可以看出，當相機存在(-1 m，1 m)范圍內(nèi)的位置誤差或(-10°，10°)范圍內(nèi)的姿態(tài)誤差時，誤差函數(shù)的形狀為碗狀，具有很好的收斂性，并且收斂點位于相機位姿真值附近，說明本文提出的優(yōu)化模型在相機位姿單獨存在較大的位置誤差或姿態(tài)誤差時仍能保證誤差函數(shù)收斂到真值，并不會出現(xiàn)收斂到局部最小值、錯誤真值的情況。

進一步探討相機位置與姿態(tài)同時存在誤差時對誤差函數(shù)產(chǎn)生的影響，將相機姿態(tài)同時加上1°或10°，畫出相機xy、xz 位置組合在一定范圍內(nèi)變化時誤差函數(shù)的變化情況，如圖4 所示。

圖4 相機姿態(tài)存在誤差時誤差函數(shù)隨相機位置誤差的變化Fig.4 Surface of error function with changes of camera position when camera pose error is existed

從圖4 中可以看出，當相機姿態(tài)存在較小誤差時，相機位置在一定范圍內(nèi)變化并不影響誤差函數(shù)的收斂性，而當姿態(tài)誤差增大時，會使得誤差函數(shù)的碗狀變得更加平滑，收斂性變?nèi)?，雖然仍能保持一定的收斂性，但收斂點已經(jīng)發(fā)生偏移，不再收斂到相機位姿真值。

綜上所述，相機位置誤差對誤差函數(shù)收斂性的影響較小，而姿態(tài)誤差對誤差函數(shù)收斂性的影響較大，會增加位置變化對誤差函數(shù)造成的影響，并且使收斂性變差、收斂點發(fā)生偏移，所以在使用本文算法解算相機與目標之間的相對位姿時，應盡量保證相機姿態(tài)初值的準確。

1. 4 位姿優(yōu)化模型的求解

通過對模型的收斂性分析可知位姿優(yōu)化模型在一定范圍內(nèi)滿足凸函數(shù)條件，因此可將基于多圓形靶標測量末端相機位姿問題歸納為關于上述非線性殘差函數(shù)的非線性最小二乘問題，最小化空間圓圓心到輪廓點的距離與圓形半徑之間的誤差，即

式中：Ω=[x，y，z，θx，θy，θz]T分別表示優(yōu)化過程中末端相機的位置和姿態(tài)參數(shù)；N 表示多圓形靶標中圓形目標的個數(shù)；Mi表示第i個圓形目標的輪廓點的個數(shù)。

上述典型的非線性最小二乘問題可通過Levenberg-Marquardt（L-M）法迭代求解，考慮到優(yōu)化模型對相機姿態(tài)誤差更加敏感，因此需要在迭代過程中增加對姿態(tài)誤差的懲罰權重，以保證相機姿態(tài)收斂的準確性。L-M 法求解流程如算法1所示。其中，L-M 法第k次迭代的殘差函數(shù)矩陣定義 為；第k 次的殘差函數(shù)矩陣對Ω 中6個變量求偏導的雅可比矩陣定義為，即

算法1 L-M 法求解流程輸入步驟1步驟2步驟3基于機械臂當前關節(jié)構型和基座位姿經(jīng)正運動學結算得到末端相機初始迭代值 Ω0=[x0，y0，z0，θx0，θy0，θz0]T、常量β、σ ∈(0，1)，當前迭代步數(shù)k=0、收斂閾值ε 取非常小的值，s 表示Ω 的長度，即s=6。取Ω=Ωk 計算殘差函數(shù)矩陣ek(Ωk)及其雅可比矩陣Jk(Ωk)。計算gk=Jk·ek，如果norm(gk)≤ε，迭代結束并返回Ω=Ωk，否則進入步驟3。計算殘差函數(shù)矩陣下降方向dk=-(JT k ·Jk+muk Es×s)-1·gk，其中muk=norm(ek)。步驟4采用Armijo 算法線性搜索合適的下降步長hk，滿足ek(Ωk+β∧h·dk)≤ek(Ωk)+σβ∧hg-1 k dk 的 最 小h 值。步驟5輸出更新自變量Ωk+1=Ωk+βhk dk，返回步驟2。末端相機精確位姿解Ω。

優(yōu)化模型的求解中最重要的一步就是相機初始迭代值的選取，從1.3 節(jié)的收斂性分析可知，優(yōu)化模型的收斂性與相機位置誤差關聯(lián)不大，所以當初始迭代值的姿態(tài)與相機姿態(tài)真值相差不多時，可以利用L-M 法求取末端相機的精確位姿，從中國空間技術研究院給出的數(shù)據(jù)顯示［19］，中國空間站機械臂的位置精度與姿態(tài)精度分別為45 mm、1°，位姿精度在優(yōu)化模型的收斂域內(nèi)，所以可以在相機位姿真值基礎上，將相機位置在(-45 mm，45 mm)范圍內(nèi)偏移，姿態(tài)在(-1°，1°)范圍內(nèi)偏移得到的新相機位姿作為基于機械臂當前關節(jié)構型和基座位姿經(jīng)正運動學結算得到末端相機初始迭代值。

2 實驗驗證與分析

為驗證本文算法的有效性，本節(jié)首先采用MATLAB 仿真的方式對算法定位精確性和抗擾能力進行驗證，然后通過虛擬仿真平臺獲取靶標仿真圖像開展空間目標測量定位實驗，通過與PNP 算法等靶標定位方法比較，驗證本文算法有效性和優(yōu)越性。

2. 1 仿真合成圖像

此節(jié)選用空間站機械臂適配器上安裝的2 類典型靶標［20-21］進行仿真實驗，2類靶標如圖5所示。

圖5 中國空間站上所使用的2 款靶標［20-21］Fig.5 Two types of targets used on China space station［20-21］

在MATLAB 軟件中開展仿真實驗，首先定義靶標坐標系，靶標坐標系相對于世界坐標系的平 移向量wXt設 為［7 700 mm，-846.6 mm，-1 209.1 mm］T，靶標平面相對于世界坐標系的滾 轉 角ψ、俯 仰 角γ 與 偏 航 角φ 分 別 設 為0°、-55°、90°，然 后 分 別 構 建2 組 離 散 點 序 列 組 成2 個合作靶標，2 個靶標中各圓形目標與靶標坐標系原點的相對位置tXti、圓形目標半徑ri按照一定比例設置。定義Ω=[x，y，z，θx，θy，θz]T為相機在世界坐標系下的位姿參數(shù)，K 為仿真相機的內(nèi)參矩陣，設置如下：

根據(jù)透視投影關系將靶標輪廓點變換到像平面上得到靶標輪廓像素坐標，相機位姿初始迭代值的選取依據(jù)1.4 節(jié)所述，將經(jīng)迭代求解出的相機位姿精確解與設置的相機位姿真值Ω 作誤差分析，定義相機光心位置誤差為Δepose=|x-姿 態(tài) 誤 差 為Δeori=|θx-，將相機的x 坐標固定在7 660 mm 不動，然后控制相機在y-z 方向從［-1 625 mm，-1 860 mm］逐漸向后移動2 m 以模擬相機在不同距離環(huán)境下對2 種靶標的識別與定位，取-200 mm 為每次測試的遞增距離，對于每組測試，取100 次獨立實驗的位置和姿態(tài)誤差均值作為實驗結果，如圖6和圖7所示。

圖6 相機在y-z 方向移動時相機位置和姿態(tài)的誤差（第1 種靶標）Fig.6 Camera position and pose errors when camera moves in y-z direction（the first target）

圖7 相機在y-z 方向移動時相機位置和姿態(tài)的誤差（第2 種靶標）Fig.7 Camera position and pose errors when camera moves in y-z direction（the second target）

從圖6 和圖7 中可知，當相機與靶標的距離逐漸增大時，利用本文提出算法解算出的相機位置誤差也逐漸增大，但姿態(tài)誤差卻逐漸減小，結合殘差函數(shù)收斂性對此原因進行分析，這是因為遠距離時，姿態(tài)存在較小的誤差會引起更大的位置誤差，所以迭代過程中為保證位置的精度，姿態(tài)誤差會略微減小，并且y-z 方向的位置誤差增長近似對稱，是因為相機光軸方向指向y-z 之間，當相機在y-z 方向移動時可以近似為在光軸方向移動，即在兩方向產(chǎn)生的誤差相近。另外通過實驗比對分析可知，因第1 種靶標使用的圓個數(shù)（4 個）比第2 種靶標（9 個）少，所以第1 種靶標的位置誤差較大，但針對上述2 種靶標，當在相機光軸方向且相機和靶標相距2 m 以內(nèi)時，相機位置與姿態(tài)的平均誤差分別＜6 mm、0.4°，具有較高的測量精度。

為驗證本文算法對強背景噪聲等干擾的抵抗能力，下面仿真實驗中引入高斯白噪聲，與PNP 算法比對定位精度情況。首先構造第1種合作靶標空間離散點在相機圖像上經(jīng)修正后的投影像素坐標歸一化前分別加入標準差為1～5 的高斯白噪聲，用于模擬不同干擾情況下對目標透視投影過程產(chǎn)生的影響，然后將帶有噪聲的像素坐標代入本文算法與橢圓擬合+EPNP 算法分別求解相機位姿，其中橢圓擬合部分是用擬合靶標離散圓環(huán)點來代替，EPNP 算法則是通過第1種靶標的4 個圓形目標的擬合圓心進行計算。為分析不同方法的位姿測量誤差，每種噪聲環(huán)境下進行100 次獨立的實驗，求取100 次的平均位置誤差平均姿態(tài)誤差，其中[ x，y，z，θx，θy，θz]T=[7 660 mm，-1 825 mm ，-1 860 mm，0°，-55°，90°]T實驗結果如圖8所示。

圖8 本文算法與EPNP 算法在不同噪聲環(huán)境下的位置誤差與姿態(tài)誤差比較（第1 種靶標）Fig.8 Comparison of position error and pose error between proposed algorithm and EPNP algorithm in different noise environments （the first target）

從圖8 中可以看出，在計算四圓靶標與相機的相對位姿時，本文算法的抗干擾性要遠遠強于EPNP 算法。同樣地，針對第2 種靶標，在相同噪聲環(huán)境下使用本文算法以及橢圓擬合+PNP 算法分別對相機位姿進行求解，因第2 種靶標的圓數(shù)量更多，所以PNP 算法分別使用基于最小二乘優(yōu)化重投影誤差的Iterative 迭代法與EPNP 算法。在解算相機位姿時，因EPNP 算法可以使用任意數(shù)量的特征點來估計位姿，所以本文算法和EPNP 算法使用9 個圓形目標，而Iterative 迭代法僅使用共面的4 個特征點，實驗結果如圖9所示。

圖9 本文算法與2 種PNP 算法在不同噪聲環(huán)境下的位置誤差與姿態(tài)誤差比較（第2種靶標）Fig.9 Comparison of position error and pose error between proposed algorithm and two types of PNP algorithms in different noise environments （the second target）

從圖9 中可以看出，當空間點透視投影到圖像平面不存在噪聲或存在很小的噪聲干擾時，本文算法與EPNP 算法解算的相機位姿均具有較高精度，而Iterative 迭代法表現(xiàn)不佳；隨著噪聲的不斷增大，Iterative 迭代法的位置誤差與姿態(tài)誤差急劇上升，在標準差＝5 時的位置與姿態(tài)誤差分別達到230 mm 與17°，抗干擾性最差；使用更多的冗余信息的EPNP 法抗干擾性優(yōu)于Iterative迭代法，但是當加入的噪聲標準差≥3 時，求取的位姿誤差也在大幅增加；與2 種PNP 算法相比，本文算法在弱噪聲環(huán)境下有著較高精度，在噪聲逐步增強的情況下誤差曲線平穩(wěn)上升，在標準差＝5 時3 個方向的位置與姿態(tài)誤差分別＜15 mm 與1.8°，即單方向位置誤差≤5 mm，單方向姿態(tài)誤差≤0.6°，具有極強的抗干擾性。

結合圖8 與圖9 分析，EPNP 算法在使用不同數(shù)量的特征點時，計算精度有所不同，其中對于特征點數(shù)目更多的第2 靶標，EPNP 算法的抗干擾性得到一定提升，而本文算法在2 種靶標下計算得到的位姿均具有較高的精度。根據(jù)以上實驗可以得出結論，在無噪聲或噪聲較小的情況下，使用四圓靶標、九圓靶標計算得出的位姿精度相當，且均略高于同實驗條件下的PNP 算法，而在噪聲較大的環(huán)境下，使用四圓靶標計算出的位姿精度最高、抗擾性最強，九圓靶標的精度稍差，但也強于PNP 算法。

2. 2 虛擬仿真平臺

為更好地驗證本文算法的有效性與精度，搭建了虛擬仿真平臺，構建與空間站核心艙機械臂適配器上相似的合作靶標，靶標及編號如圖10 所示，其中1 號圓的白色外圈半徑最大，中間白色圓形外圈半徑最小，其他2～8 號的白色外圈半徑一致，合作靶標安裝在適配器左上角，且靶標上的每個圓形目標與適配器之間的相對位姿關系已知，如表1 所示。

表1 靶標各圓形目標尺寸及與適配器之間的相對位姿關系Table 1 Dimensions of each circular target and relative positional relationship between target and adapter

圖10 多圓形合作靶標與適配器樣式Fig.10 Type of multi-circular cooperative target and adapter

然后創(chuàng)建內(nèi)參fu和fv已知的相機仿真模型，為模擬真實情況下相機對合作靶標的拍攝情況，利用相機從9 個不同方位由遠及近地對靶標區(qū)域進行拍攝，每個方向獲取2 張距離不一的合作靶標圖像總計18 張圖像，如圖11 所示。

圖11 從不同方位、不同距離獲取的合作靶標圖像Fig.11 Images of cooperative targets acquired from different orientations and distances

為精確獲取圖像中靶標的所在區(qū)域，減少合作靶標檢測的無效干擾，使用Halcon 形狀匹配算法［22］來識別圖像中的合作靶標并對其最小外接矩形進行截取，如圖12 所示。Halcon 形狀匹配的原理是結合合作靶標的邊緣特征與灰度信息來創(chuàng)建模板，然后在待匹配圖像的圖像金字塔中逐層搜索模板圖像至最底層或得到確定的匹配結果為止。

圖12 多圓形靶標區(qū)域提取結果Fig.12 Multi-circular target area extraction results

在得到裁剪過的靶標區(qū)域后，使用Canny 邊緣檢測算法來提取合作靶標上圓形目標的輪廓像素點u(i，j)(i=1，2，…，8)并通過人工分組的方式將所有檢測到的輪廓點分別歸入到9 個圓形目標，在靶標被遮擋或在視角范圍外時，提取剩余圓形目標的輪廓像素。在仿真平臺中，將適配器設為靶標坐標系原點，其在世界坐標系下的位姿設置與 2.1 節(jié)仿真實驗一致，即wXt=[7 700 mm，-846.6 mm，-1 209.1 mm ]T，適配器相對于世界坐標系旋轉的三角度為[ψ，γ，φ]T=[0°，-55°，90°]T。各圓環(huán)圓心的世界坐標wXti由表2 與wTt計算可得。

表2 視覺定位部分實驗中本算法與2 類PNP 算法的耗時比較Table 2 Comparison of time consumption between our algorithm and two types of PNP algorithms in part of visual localization experiment

圖13 為針對上述18 張合作靶標圖像使用本文算法解算的世界坐標系下的相機位姿與真值之間的誤差。其中位置誤差由x 方向誤差Δx、y 方向誤差Δy 與z 方向誤差Δz 表示，姿態(tài)誤差由滾轉角誤差Δθx、俯仰角誤差Δθy與偏航角誤差Δθz表示。

圖13 利用本文提出算法計算的相機位姿誤差Fig.13 Camera position error and pose error calculated using our algorithm

從仿真實驗結果可知，本文算法解算的不同距離相機位姿均具有較高精度，其中單方向的位置誤差≤25 mm，姿態(tài)誤差≤1°，因仿真平臺中拍攝的圖像存在一定的圖像噪聲與輪廓像素提取誤差，即使是使用相機真值代入到優(yōu)化模型中計算出的殘差也不為0，所以將迭代收斂閾值ε 設為10 mm，此情況下則會導致相機距離目標更遠情況下的定位精度可能略高于相機距離目標更近情況下的精度。

為驗證改進算法在精度與性能上的優(yōu)勢，同樣使用橢圓擬合+PNP 算法對這18 組圖像的相機位姿進行解算并與本文算法進行比較，其中橢圓擬合使用的是最小二乘法來對離散輪廓點進行擬合，PNP 方法使用的是Iterative 迭代法與EPNP 算法。實驗采用QT 5.9.0 編譯器，邊緣檢測、橢圓擬合、2 種PNP 算法與本文算法均用C++語言編程，其中Iterative 迭代法與EPNP 算法采用OpenCV 3.2.0 版本的內(nèi)置函數(shù)，本文算法基于Eigen 矩陣運算庫實現(xiàn)，程序運行計算機配置為Intel（R） Core（TM） i7-5500U CPU @ 2.40 GHz，8G RAM。

圖14 為針對上述18 張合作靶標圖像數(shù)據(jù)使用本算法、Iterative 迭代法與EPNP 算法分別解算的世界坐標系下的相機位姿與相機位姿真值之間的誤差。由于存在靶標圓環(huán)被遮擋、靶標位于相機視角外等情況，所以Iterative 迭代法使用的是經(jīng)邊緣檢測算法成功提取到的前4 個共面圓環(huán)輪廓點擬合圓心，EPNP 算法使用的是經(jīng)邊緣檢測算法成功提取到的所有圓環(huán)輪廓點擬合圓心。

圖14 本文算法與2 種PNP 算法的相機位姿誤差比較Fig.14 Comparison of position error and pose error between proposed algorithm and two types of PNP algorithms

從圖14 中可以看出，本文算法在相機從不同視角、不同距離拍攝合作靶標的情況下均取得較高精度，也比相同條件下的PNP 算法表現(xiàn)更佳，PNP 算法在靶標距離相機較近且相對相機角度偏移不大的情況下才可能取得較高的精度，并且因本算法使用更多冗余點信息，在PNP 算法表現(xiàn)不佳的情況仍能求取高精度的相機位姿，普適性更強。位置誤差中，x 方向的位置平均誤差比其他2 個方向的平均誤差小，這是因為相機光軸指向世界坐標系y、z 軸之間，而相機在光軸方向移動時會直接造成目標與相機之間深度的變化，誤差會更大。與2.1 節(jié)MATLAB 仿真合成圖像的第2 種靶標實驗相比，虛擬仿真平臺下各算法的精度都有降低，這是因為數(shù)字虛擬平臺中需要考慮的誤差因素更多，例如圖像噪聲、相機標定誤差、邊緣提取與橢圓擬合的誤差等，與實際任務場景更為接近。

進一步對本文算法與PNP 算法的時效性進行比較。圖15 為上述18 次實驗中本算法與橢圓擬合+兩種PNP 算法的耗時比較。

圖15 18 次實驗中本文算法與橢圓擬合+2 種PNP 算法的耗時比較Fig.15 Comparison of time consumption between proposed algorithm and ellipse fitting + two types of PNP algorithms in 18 experiments

由于Iterative 迭代法與EPNP 算法所需擬合的圓心數(shù)量不同，所以橢圓擬合步驟的耗時也不相同，圖15 中的橢圓擬合+2 類PNP 算法耗時是由Iterative 迭代法或EPNP 算法對應的橢圓擬合耗時再加上其本身耗時得到。為更直觀地給出視覺定位流程中本文算法與橢圓擬合+PNP 算法定位策略時間的差異情況，每隔3 組抽取1 組實驗數(shù)據(jù)，即第1、4、7、10、13、16 次實驗數(shù)據(jù)的比對如表2 所示。

從圖15 和表2 中可以看出，橢圓擬合+Iterative 迭代法的耗時與橢圓擬合+EPNP 算法的耗時相當，而本文算法耗時遠小于橢圓擬合+2 種PNP 算法的耗時。此次實驗重點比較靶標定位最簡步驟的所需時間，假定靶標已經(jīng)完成識別提取，所以在對2 種PNP 算法計算位姿的過程中采用簡單的輪廓擬合+定位的方式。而在實際應用中，PNP 算法使用前為了提取更加準確的靶標橢圓輪廓，往往采用更為復雜的橢圓檢測算法，時間耗費會更大，因此本文算法較2種PNP 算法的時效性能方面有顯著提升。該算法在中國空間站機械臂抓捕適配器的地面監(jiān)視與實時測量任務中得到應用，在精度、穩(wěn)定性和時效性方面表現(xiàn)良好。

3 結 論

本文針對目前基于合作靶標的視覺定位中存在的靶標點提取運算復雜、易受干擾而導致時效性、穩(wěn)定性和精確性不高等問題，設計了基于多圓形合作靶標的視覺定位方法，將提取到的圓形靶標輪廓離散點逆投影至三維空間中形成多個空間射線簇，在空間中構建殘差函數(shù)實現(xiàn)目標位姿的精確求解。通過2 類實驗驗證了提出算法的有效性：

1）仿真合成圖像實驗。在MATLAB 仿真平臺上設計2 種靶標測試本文算法在不同距離、不同噪聲下的定位性能，取得較PNP 算法更好的定位精度和抗干擾性。

2）虛擬仿真平臺實驗。在虛擬仿真平臺中模擬空間機械臂在軌抓捕目標適配器的過程，利用相機從不同方位對合作靶標進行提取與定位，對不同視角拍攝的靶標圖像使用本文算法與PNP 算法對相機位姿進行解算，本文算法計算得到的相機位置與姿態(tài)均取得了較高精度，滿足中國空間站機械臂末端的視覺定位精度要求與實時性要求，驗證了本文算法的精確性及時效性。

本文算法在空間站機械臂抓捕適配器的地面監(jiān)視過程中得到應用，其精度、穩(wěn)定性和時效性均取得良好的效果。該方法可應用于星艦對接環(huán)抓捕、機械臂對光學艙抓捕等多類合作與非合作任務場景中，實現(xiàn)對單個或多個圓形目標的快速穩(wěn)定精確視覺測量，為中國航天任務的相對視覺測量能力的提升提供有力的理論與技術支撐。

致 謝

論文在北京航天飛行控制中心空間遙操作團隊的通力協(xié)助下得以順利完成，在此深表感謝。