譚啟蒙 胡成威 高升

（北京空間飛行器總體設(shè)計(jì)部，北京 100094）

1 引言

鑒于國(guó)際空間站的成功應(yīng)用經(jīng)驗(yàn)[1-2]，空間機(jī)械臂是深入開展航天活動(dòng)不可或缺的重要組成部分，其作為支持空間站組裝、運(yùn)營(yíng)、維護(hù)和空間應(yīng)用的關(guān)鍵技術(shù)手段，承擔(dān)著艙段轉(zhuǎn)位、懸停飛行器捕獲和輔助對(duì)接、艙外貨物搬運(yùn)、支持航天員出艙活動(dòng)、支持艙外狀態(tài)檢查、支持艙外大型設(shè)備維修維護(hù)更換、照料暴露平臺(tái)試驗(yàn)載荷、照料光學(xué)平臺(tái)等任務(wù)。

空間機(jī)械臂在軌工作過程中，不僅自身需要實(shí)現(xiàn)大范圍的運(yùn)動(dòng)轉(zhuǎn)移，還特別需要完成一系列如設(shè)備拆裝、貨物抓取、艙段轉(zhuǎn)位對(duì)接等高精度的精準(zhǔn)操作[3]。為滿足上述要求，空間機(jī)械臂的必備組成之一——視覺相機(jī)可以實(shí)現(xiàn)對(duì)機(jī)械臂整個(gè)工作區(qū)域的監(jiān)控和觀測(cè)目標(biāo)的三維（3D）位姿測(cè)量，為機(jī)械臂在軌運(yùn)行提供必需的圖像信息和目標(biāo)的位置、朝向以及運(yùn)動(dòng)信息。與目前各類航天器攜帶的載荷相機(jī)比較，機(jī)械臂視覺相機(jī)除兼顧以往傳統(tǒng)的監(jiān)視、觀測(cè)功能外，還須具備檢測(cè)識(shí)別觀測(cè)目標(biāo)、跟蹤與測(cè)量空間位姿等功能。視覺相機(jī)測(cè)量原理主要是根據(jù)圖像平面中的二維（2D）圖像坐標(biāo)及其所對(duì)應(yīng)的3D 空間坐標(biāo)之間的映射關(guān)系，解算出合作目標(biāo)的空間位置姿態(tài)。為建立2D 圖像坐標(biāo)與3D 空間坐標(biāo)之間的對(duì)應(yīng)關(guān)系，就必須對(duì)視覺相機(jī)實(shí)施標(biāo)定，以獲取視覺相機(jī)內(nèi)部成像模型中的所有未知參數(shù)[4]，即表征相機(jī)內(nèi)部幾何結(jié)構(gòu)的內(nèi)部參數(shù)，主要包括焦距、主點(diǎn)坐標(biāo)、畸變系數(shù)等，并為空間目標(biāo)位姿計(jì)算提供系數(shù)矩陣。因此，視覺相機(jī)內(nèi)參標(biāo)定作為空間機(jī)械臂至關(guān)重要的技術(shù)環(huán)節(jié)，是實(shí)現(xiàn)目標(biāo)3D 位姿精確測(cè)量的基本前提和重要保障。

“加拿大臂”作為人類歷史上第一個(gè)得到成功應(yīng)用的空間機(jī)械臂，其自身攜帶的空間視覺系統(tǒng)SVS（Space Vision System）早在1990年就已成功完成了視覺相機(jī)的內(nèi)參標(biāo)定任務(wù)[5-6]。其標(biāo)定原理是：利用相機(jī)在多個(gè)不同站位處，觀測(cè)同一個(gè)位置已知固定的圓形標(biāo)記點(diǎn)陣列圖案靶標(biāo)，根據(jù)前方交會(huì)原理，將所有圖像代入攝影測(cè)量共線方程式，利用光束平差法解算出相機(jī)內(nèi)方位參數(shù)值，從而完成內(nèi)參標(biāo)定。該標(biāo)定方法標(biāo)定精度高，但其不足之處在于：必須嚴(yán)格規(guī)定靶標(biāo)的位置；在共線方程組求解與光束平差過程中，需要使用非線性優(yōu)化算法，從而引入過多的非線性參數(shù)，這樣不但不會(huì)提高標(biāo)定精度，反而會(huì)引起解的不穩(wěn)定性。針對(duì)這一問題，本文提出了一種基于2D 平面靶的空間機(jī)械臂相機(jī)內(nèi)參標(biāo)定方法，對(duì)靶標(biāo)的位置不作特殊要求，即平面靶可在相機(jī)視場(chǎng)范圍內(nèi)隨意運(yùn)動(dòng)；在內(nèi)參求解中，實(shí)施分步計(jì)算，先利用線性成像模型計(jì)算焦距與主點(diǎn)初值，再引入畸變系數(shù)建立非線性成像模型，并利用非線性優(yōu)化算法獲取內(nèi)參最優(yōu)解，如此既能保證較高的標(biāo)定精度，又能夠確保最優(yōu)解處于收斂狀態(tài)，且有效簡(jiǎn)化了目標(biāo)函數(shù)的計(jì)算難度。

2 空間機(jī)械臂視覺相機(jī)

2.1 視覺相機(jī)配置

視覺相機(jī)主要安裝在機(jī)械臂的末端執(zhí)行機(jī)構(gòu)上，其采用單目測(cè)量方式，通常需要配置光源以增強(qiáng)、補(bǔ)償相機(jī)對(duì)弱光照或惡劣光照條件的適應(yīng)性，如圖1所示。視覺相機(jī)的功能主要表現(xiàn)在以下幾個(gè)方面：1）實(shí)時(shí)監(jiān)控相機(jī)視場(chǎng)范圍內(nèi)的可操作區(qū)域；2）實(shí)時(shí)檢測(cè)、識(shí)別與跟蹤近距離處的空間合作目標(biāo)；3）精確估計(jì)近距離處的空間合作目標(biāo)的3D 空間位置姿態(tài)。

現(xiàn)以合作目標(biāo)飛行器抓取捕獲為例，簡(jiǎn)要概述視覺相機(jī)的工作流程：空間機(jī)械臂在軌執(zhí)行任務(wù)過程中，當(dāng)觀測(cè)目標(biāo)位于遠(yuǎn)距離處，機(jī)械臂按照預(yù)編程的路徑規(guī)劃，緩慢地靠近合作目標(biāo)；當(dāng)觀測(cè)目標(biāo)位于近距離處，目標(biāo)已經(jīng)成功進(jìn)入視覺相機(jī)的有效視場(chǎng)范圍內(nèi)，視覺相機(jī)能夠?qū)崟r(shí)檢測(cè)、識(shí)別與跟蹤近距離處的空間合作目標(biāo)，并精確估計(jì)近距離處的空間目標(biāo)3D 位姿，從而引導(dǎo)機(jī)械臂末端執(zhí)行機(jī)構(gòu)進(jìn)一步調(diào)整自身位置姿態(tài)以逐步減小其與空間合作目標(biāo)之間的旋轉(zhuǎn)差異和平移差異，確保機(jī)械臂末端執(zhí)行機(jī)構(gòu)能夠準(zhǔn)確捕獲、抓取、鎖定合作目標(biāo)飛行器。

圖1 空間機(jī)械臂視覺相機(jī)配置示意Fig.1 Configuration of space manipulator camera

2.2 視覺相機(jī)位姿測(cè)量原理

圖2 解釋了視覺相機(jī)位姿測(cè)量的基本原理。合作目標(biāo)攜帶的某一個(gè)標(biāo)記點(diǎn)P，該標(biāo)記點(diǎn)P 與相機(jī)光心cO 的連線與像平面之間的交點(diǎn)q 即為該點(diǎn)在視覺相機(jī)成像面上所成的對(duì)應(yīng)圖像特征點(diǎn)。則3D 空間坐標(biāo)值與2D 圖像坐標(biāo)值之間的函數(shù)關(guān)系可表示為[7]

式中 （u,v ）為點(diǎn)q 的2D 圖像坐標(biāo)，主要通過對(duì)采集圖像信息進(jìn)行亞像素圖像特征點(diǎn)中心定位算法檢測(cè)得到；（X0,Y0,Z0）為標(biāo)記點(diǎn)P 在目標(biāo)坐標(biāo)系下的3D 空間坐標(biāo)，通常利用電子經(jīng)緯儀或激光跟蹤儀等高精度測(cè)量?jī)x器實(shí)施精密測(cè)量得到[8]，可視為已知量；A為視覺相機(jī)成像模型的內(nèi)參矩陣，包括焦距、主點(diǎn)坐標(biāo)、畸變系數(shù)等，主要依賴于視覺相機(jī)的內(nèi)參標(biāo)定結(jié)果；R,T 分別為旋轉(zhuǎn)矩陣和平移矢量，為待求未知參數(shù)，形象描述了空間合作目標(biāo)的包含6個(gè)自由度信息的相對(duì)空間位置姿態(tài)關(guān)系。

圖2 視覺相機(jī)位姿測(cè)量原理示意Fig.2 Principle of estimation of position and orientation

3 視覺相機(jī)內(nèi)參標(biāo)定技術(shù)

空間機(jī)械臂視覺相機(jī)內(nèi)參標(biāo)定的目的在于準(zhǔn)確計(jì)算焦距、主點(diǎn)坐標(biāo)、畸變系數(shù)等相機(jī)內(nèi)部固有參數(shù)，主要設(shè)計(jì)一種基于2D 平面靶的內(nèi)參標(biāo)定方法。與張正友標(biāo)定方法相類似[9-10]，該法假定相機(jī)內(nèi)部參數(shù)在整個(gè)標(biāo)定過程中始終保持不變，即不論相機(jī)從任何角度拍攝標(biāo)定靶，相機(jī)內(nèi)參均為常數(shù)，只有外部參數(shù)發(fā)生改變。通過采集二維平面靶在不同方位下的多幅圖像，利用平面靶和圖像之間的點(diǎn)對(duì)應(yīng)關(guān)系即可得到平面靶與圖像平面之間的對(duì)應(yīng)矩陣，即可得到相機(jī)內(nèi)參的約束方程，利用非線性優(yōu)化算法，從而解算出相機(jī)內(nèi)部參數(shù)最優(yōu)值。

基于2D 平面靶的視覺相機(jī)內(nèi)參標(biāo)定方法的數(shù)學(xué)計(jì)算過程具體如下。

假設(shè)平面標(biāo)定靶上的某一個(gè)標(biāo)記點(diǎn)M 的3D 空間坐標(biāo)為(X,Y,Z)，其在相機(jī)圖像平面上的對(duì)應(yīng)特征點(diǎn)m 的2D 圖像坐標(biāo)為(u,v)，點(diǎn)m和M 的齊次坐標(biāo)分別表示為(u,v,1)，(X,Y,Z,1)。根據(jù)視覺相機(jī)線性成像模型，3D 空間點(diǎn)M 與2D 圖像點(diǎn)m 之間的映射關(guān)系為[7]

式中 (u0,0v)為主點(diǎn)坐標(biāo)；α和β 分別為水平和垂直方向的等效焦距；γ為圖像坐標(biāo)軸的偏斜因子。

3.1 平面靶與相機(jī)圖像之間的映射矩陣

不失一般性，以標(biāo)定平面作為目標(biāo)空間坐標(biāo)系的xy 平面來建立目標(biāo)坐標(biāo)系，那么標(biāo)定靶上特征點(diǎn)坐標(biāo)滿足：Z=0，記旋轉(zhuǎn)矩陣R=[r1r2r3]，由式（2）得

此時(shí)，用M=[X Y]T來代替M=[X Y Z ]T，記M～=[X Y1]T，平面標(biāo)定靶上點(diǎn)M和對(duì)應(yīng)像點(diǎn)m 之間可以通過映射矩陣H 來轉(zhuǎn)換：

映射矩陣H 可以通過使得像點(diǎn)的重投影誤差最小的優(yōu)化方法來計(jì)算得到[9]，優(yōu)化目標(biāo)函數(shù)為

3.2 相機(jī)內(nèi)參約束條件

設(shè)H=[h1h2h3]，從式（5）可以得出 [h1h2h3]=λA[r1r2t]，此處的λ 是比例系數(shù)，根據(jù)R的正交性[7]，可以得到為相機(jī)內(nèi)參的兩個(gè)基本約束條件：

3.3 內(nèi)參矩陣求解

因?yàn)锽 是對(duì)稱矩陣，可以用六維的矢量b 來表示。

設(shè)矩陣H 的第i 列向量為hi=[hi1hi2hi3]T，則有

因此，可以將相機(jī)內(nèi)參的兩個(gè)基本約束關(guān)系式表示為

式（9）中，b為六維向量，因此，至少需要6個(gè)方程才能進(jìn)行求解。移動(dòng)平面標(biāo)定靶至不同的位置，可以得到n（n≥3）幅圖像，對(duì)應(yīng)得到2n個(gè)方程，組合起來表示為

方程（12）的解對(duì)應(yīng)于矩陣VTV最小特征值對(duì)應(yīng)的特征向量。一旦b 求解出來，就可以通過Cholesky矩陣分解計(jì)算得到相機(jī)的內(nèi)參矩陣A。

根據(jù)式（13）計(jì)算出的上述參數(shù)值對(duì)應(yīng)了式（3）表示的內(nèi)參矩陣A 的所有分量，這些參數(shù)即為相機(jī)標(biāo)定的全部?jī)?nèi)參。

3.4 非線性模型

實(shí)際上，由于相機(jī)光學(xué)系統(tǒng)設(shè)計(jì)和加工誤差而引起的圖像幾何變形較大，需要引入非線性的相機(jī)模型[10]。

相機(jī)的非線性畸變可以表示為

式中 （xu,yu）為由相機(jī)線性模型計(jì)算出來的圖像點(diǎn)坐標(biāo)；（xd,yd）為實(shí)際圖像點(diǎn)的坐標(biāo)；δx和 δy為非線性畸變值，它與圖像點(diǎn)在圖像中的位置有關(guān)，可表示為

其中，δx和 δy的第一組多項(xiàng)式稱為徑向畸變，第二組多項(xiàng)式稱為切向畸變。式（15）中，k1,k2,k3,p1,p2通常簡(jiǎn)稱為（非線性）畸變系數(shù)。

最后，通過建立優(yōu)化目標(biāo)函數(shù)：

4 試驗(yàn)結(jié)果與分析

視覺相機(jī)內(nèi)參標(biāo)定試驗(yàn)旨在確定焦距、主點(diǎn)坐標(biāo)、畸變系數(shù)等成像參數(shù)?，F(xiàn)以空間機(jī)械臂視覺相機(jī)為例，完成相機(jī)內(nèi)參標(biāo)定試驗(yàn)。具體操作過程如下：

首先，將帶有棋盤格圖案的平面標(biāo)定靶置于視覺相機(jī)的視場(chǎng)范圍內(nèi)；然后，利用相機(jī)先后拍攝平面靶在40個(gè)不同位置、姿態(tài)處的圖像；隨后，將所有采集后的標(biāo)定靶圖像分別代入上述基于2D 平面靶的標(biāo)定算法中進(jìn)行處理，根據(jù)靶面與圖像平面之間的特征角點(diǎn)對(duì)應(yīng)關(guān)系即可得到平面靶所在平面與相機(jī)圖像平面之間的對(duì)應(yīng)矩陣，以此建立若干相機(jī)內(nèi)參的約束方程；最后，利用Levenberg-Marquardt 優(yōu)化算法解算出相機(jī)內(nèi)參矩陣最優(yōu)解。

將上述40 幅不同位置處的平面靶圖像分別代入內(nèi)參標(biāo)定算法進(jìn)行計(jì)算，可得內(nèi)參標(biāo)定結(jié)果（如表1所示）。

表1 視覺相機(jī)內(nèi)參標(biāo)定試驗(yàn)結(jié)果Table 1 Experimental result for calibration of camera intrinsic parameters

表1 列出了視覺相機(jī)內(nèi)參矩陣A 中的焦距、主點(diǎn)坐標(biāo)、畸變系數(shù)等不確定量，可視為相機(jī)內(nèi)部參數(shù)的標(biāo)定精度與誤差水平。水平、垂直方向的歸一化焦距的不確定量均近似于1mm，主點(diǎn)坐標(biāo)不確定量不超過1個(gè)像素，畸變系數(shù)不確定量基本可忽略不計(jì)。鑒于上述分析可知，文中設(shè)計(jì)的一種基于2D 平面靶的內(nèi)參標(biāo)定方法可適用于機(jī)械臂相機(jī)內(nèi)參標(biāo)定，標(biāo)定結(jié)果真實(shí)可靠，符合空間機(jī)械臂視覺相機(jī)獲取合作目標(biāo)3D 位姿的測(cè)量精度要求，可直接用于解算觀測(cè)目標(biāo)的位置、姿態(tài)。

5 結(jié)束語

為適應(yīng)空間機(jī)械臂的實(shí)際任務(wù)需求，視覺相機(jī)不僅需要兼顧以往各類航天器攜帶的載荷相機(jī)的監(jiān)視和觀測(cè)功能，還必須具備觀測(cè)目標(biāo)檢測(cè)識(shí)別、空間位姿跟蹤與測(cè)量功能，也就是說，它能為機(jī)械臂在軌運(yùn)行提供必需的圖像信息和目標(biāo)的位置、朝向以及運(yùn)動(dòng)信息。根據(jù)合作目標(biāo)空間位姿測(cè)量基本原理，相機(jī)內(nèi)參標(biāo)定的目的是提供一組表征相機(jī)成像模型的可靠的內(nèi)參矩陣，主要涉及焦距、主點(diǎn)坐標(biāo)、畸變系數(shù)等，它是實(shí)現(xiàn)目標(biāo)3D 位姿精確測(cè)量的重要保障。文中設(shè)計(jì)一種基于2D 平面靶的視覺相機(jī)內(nèi)參標(biāo)定方法有效解決了“加拿大臂”中視覺相機(jī)內(nèi)參標(biāo)定的計(jì)算復(fù)雜性等問題。該方法具有標(biāo)定精度高、計(jì)算簡(jiǎn)便快捷等優(yōu)點(diǎn)，主要表現(xiàn)為兩個(gè)方面：1）該方法完全不受相機(jī)站位和靶標(biāo)位置的限制，即平面靶可在相機(jī)視場(chǎng)范圍內(nèi)隨意運(yùn)動(dòng)；2）求解內(nèi)參約束方程時(shí)，實(shí)施分步計(jì)算，先利用特征值分解計(jì)算出相機(jī)線性成像模型中的焦距與主點(diǎn)坐標(biāo)初值，再考慮引入徑向畸變和切向畸變構(gòu)建非線性模型，并利用Levenberg-Marquardt 非線性優(yōu)化算法求解，可得到焦距、主點(diǎn)坐標(biāo)、畸變系數(shù)等相機(jī)內(nèi)參的最優(yōu)解?？臻g機(jī)械臂視覺相機(jī)標(biāo)定試驗(yàn)中，焦距標(biāo)定誤差近似于1mm，主點(diǎn)坐標(biāo)誤差不超過1個(gè)像素，所有畸變系數(shù)誤差均忽略不計(jì)。因此，基于2D 平面靶的內(nèi)參標(biāo)定方法適用于機(jī)械臂相機(jī)內(nèi)參標(biāo)定，且標(biāo)定結(jié)果均符合機(jī)械臂相機(jī)獲取目標(biāo)3D 位姿的精度要求，可直接用于解算觀測(cè)目標(biāo)的位置、姿態(tài)等后續(xù)任務(wù)。

References)

[1]于登云,孫京,馬興瑞.空間機(jī)械臂技術(shù)及發(fā)展建議[J].航天器工程,2007,16(4):1-8.YU Dengyun,SUN Jing,MA Xingrui.Suggestion on Development of Chinese Space Manipulator Technology[J].Spacecraft Engineering,2007,16(4):1-8.(in Chinese)

[2]秦文波,陳萌,張崇峰,等.空間站大型機(jī)構(gòu)研究綜述[J].上海航天,2010,(4):32-42.QIN Wenbo,CHEN Meng,ZHANG Chongfeng,et al.Surveys on Large-Scale Mechanism of Space Station[J].Aerospace Shanghai,2010,(4):32-42.(in Chinese)

[3]張凱鋒,周暉,溫慶平,等.空間站機(jī)械臂研究[J].空間科學(xué)學(xué)報(bào),2010,30(6):612-619.ZHANG Kaifeng,ZHOU Hui,WEN Qingping,et al.Review of the Development of Robotic Manipulator for International Space Station[J].Chinese Journal of Space Science,2010,30(6):612-619.(in Chinese)

[4]http://www.asc-csa.gc.ca/eng/default.asp.

[5]http://www.asc-csa.gc.ca/eng/canadarm/vision.asp.

[6]G.MacLean S,Pinkey H F L.Calibration Procedures for the Space Vision System Experiment[J].Industrial Vision Metrology,1991,1526:113-122.

[7]張廣軍.視覺測(cè)量[M].北京:科學(xué)出版社,2008.ZHANG Guangjun.Vision Measurement[M].Beijing:Science Press,2008.(in Chinese)

[8]李廣云.工業(yè)測(cè)量系統(tǒng)最新進(jìn)展及應(yīng)用[J].測(cè)繪工程,2001,10(2):36-40.LI Guangyun.The State of the Art and Applications of the Industrial Measuring Systems[J].Engineering of Surveying and Mapping,2001,10(2):36-40.(in Chinese)

[9]ZHAGN Zhengyou.A Flexible New Technique for Camera Calibration[J].IEEE Trans.on Pattern Analysis and Machine Intelligence,2000,22(11):1330-1334.

[10]馬頌德,張正友.計(jì)算機(jī)視覺——計(jì)算理論與算法基礎(chǔ)[M].北京:科學(xué)出版社,2003.MA Songde,ZHANG Zhengyou.Computer Vision——Computational Theory and Algorithm[M].Beijing:Science Press,2003.(in Chinese)

[11]Marquardt D W.An Algorithm for Least-squares Estimation of Nonlinear Parameters[J].Journal of the Society for Industrial and Applied mathematics,1963,11(2):431-441.

[12]Madsen K,Nielsen H B,Tingleff O.Methods for Non-linear Least Squares Problems[M].Copenhagen:Informatics and Mathemetical Modelling Technical University of Denmark,2ndEdition,2004.

[13]Jose Pujol.The Solution of Non-linear Inverse Problems and The Levenberg-Marquardt Method[J].Geophysics,2007,72(4):W1-W16.