基于單目視覺的天線副面并聯(lián)調(diào)整機構(gòu)相對位姿測量方法*

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

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

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

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

1 引言

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

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

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

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

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

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

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

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

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

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

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

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

(3)單目視覺位姿解算

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

3 單目視覺位姿測量原理

3.1 單目視覺位姿測量模型

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

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

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

3.2 位姿解算算法

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

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

3.3 圖像特征點中心坐標(biāo)的提取

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

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

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

4 實驗和結(jié)果分析

4.1 實驗描述

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

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

圖5 實驗平臺Fig.5 Experimental platform

圖6 相機和合作目標(biāo)Fig.6 Camera and cooperation target

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

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

4.2 結(jié)果分析

通過改變六自由度并聯(lián)機構(gòu)動平臺的平移距離和旋轉(zhuǎn)角度， 將合作目標(biāo)相對于相機的位置和姿態(tài)參數(shù)作為測量值， 然后拍攝圖像測試方法的有效性. 以垂直于相機光軸位置為零位， 選擇的平移范圍為±30 mm，依次沿x、y、z軸平移5 mm;旋轉(zhuǎn)范圍為±5°， 依次沿x、y、z軸旋轉(zhuǎn)1°. 將測量值與理論值進行比較， 得到平移距離和旋轉(zhuǎn)角度的位姿測量誤差曲線， 結(jié)果如圖8所示; 對圖8進行數(shù)據(jù)分析，分別得到平移距離誤差表和旋轉(zhuǎn)角度誤差表， 如表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可知， 當(dāng)動平臺偏離靜平臺時， 沿x軸和y軸的平移和旋轉(zhuǎn)方向上的測量誤差呈現(xiàn)相對增大趨勢， 主要原因是合作目標(biāo)逐漸偏離相機的測量范圍， 降低了圖像點的提取精度， 從而使圖像處理誤差增大; 沿z軸平移方向上的測量誤差從零位開始逐漸增大， 相對于其他方向的誤差，z方向較大， 主要原因是動平臺的抖動和景深增大， 前者在動平臺沿z軸上下移動時導(dǎo)致合作目標(biāo)偏離原本固定于動平臺的位置， 使得解算出來的位姿出現(xiàn)誤差， 而后者降低了圖像點的提取精度; 但沿z軸的旋轉(zhuǎn)誤差相對較小， RMSE也較小， 主要由于水平轉(zhuǎn)動不影響圖像處理精度. 由實驗數(shù)據(jù)可知， 相機與合作目標(biāo)間平移±30 mm范圍內(nèi)， 測量誤差小于0.1 mm. 旋轉(zhuǎn)±5°范圍內(nèi)， 測量誤差小于0.05°， 該測量精度滿足六自由度并聯(lián)機構(gòu)的精度要求.

圖8 位姿測量誤差: (a) x軸平移誤差; (b) y軸平移誤差; (c) z軸平移誤差; (d) α旋轉(zhuǎn)角誤差; (e) β旋轉(zhuǎn)角誤差; (f) γ旋轉(zhuǎn)角誤差.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 旋轉(zhuǎn)角度測量誤差Table 2 Rotation angle measurement error

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

5 結(jié)論

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