張文哲 項(xiàng)斌斌 王娜 王兆軍 古麗加依娜哈再孜汗，3

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

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

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

1 引言

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

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

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

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

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

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

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

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

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

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

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

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

(3)單目視覺位姿解算

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

3 單目視覺位姿測(cè)量原理

3.1 單目視覺位姿測(cè)量模型

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

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

設(shè)點(diǎn)P在相機(jī)坐標(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)定獲得相機(jī)的內(nèi)參矩陣為A， 圖像主點(diǎn)坐標(biāo)為O0= (u0，v0)，則P點(diǎn)在彼此坐標(biāo)系下的關(guān)系為:

3.2 位姿解算算法

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

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

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

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

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

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

4 實(shí)驗(yàn)和結(jié)果分析

4.1 實(shí)驗(yàn)描述

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

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

圖5 實(shí)驗(yàn)平臺(tái)Fig.5 Experimental platform

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

圖7 特征點(diǎn)圖像處理圖Fig.7 Image processing chart of feature points

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

4.2 結(jié)果分析

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

由表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)動(dòng)平臺(tái)偏離靜平臺(tái)時(shí)， 沿x軸和y軸的平移和旋轉(zhuǎn)方向上的測(cè)量誤差呈現(xiàn)相對(duì)增大趨勢(shì)， 主要原因是合作目標(biāo)逐漸偏離相機(jī)的測(cè)量范圍， 降低了圖像點(diǎn)的提取精度， 從而使圖像處理誤差增大; 沿z軸平移方向上的測(cè)量誤差從零位開始逐漸增大， 相對(duì)于其他方向的誤差，z方向較大， 主要原因是動(dòng)平臺(tái)的抖動(dòng)和景深增大， 前者在動(dòng)平臺(tái)沿z軸上下移動(dòng)時(shí)導(dǎo)致合作目標(biāo)偏離原本固定于動(dòng)平臺(tái)的位置， 使得解算出來的位姿出現(xiàn)誤差， 而后者降低了圖像點(diǎn)的提取精度; 但沿z軸的旋轉(zhuǎn)誤差相對(duì)較小， RMSE也較小， 主要由于水平轉(zhuǎn)動(dòng)不影響圖像處理精度. 由實(shí)驗(yàn)數(shù)據(jù)可知， 相機(jī)與合作目標(biāo)間平移±30 mm范圍內(nèi)， 測(cè)量誤差小于0.1 mm. 旋轉(zhuǎn)±5°范圍內(nèi)， 測(cè)量誤差小于0.05°， 該測(cè)量精度滿足六自由度并聯(lián)機(jī)構(gòu)的精度要求.

圖8 位姿測(cè)量誤差: (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 平移距離測(cè)量誤差Table 1 Translational distance measurement error

表2 旋轉(zhuǎn)角度測(cè)量誤差Table 2 Rotation angle measurement error

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

5 結(jié)論

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