賈慶軒，段嘉琪，陳鋼

北京郵電大學(xué) 自動化學(xué)院，北京 100876

隨著對太空開發(fā)的不斷深入，空間結(jié)構(gòu)的大型化是未來航天事業(yè)發(fā)展的主要趨勢。許多科學(xué)研究計(jì)劃都要求有大型的空間結(jié)構(gòu)作為基礎(chǔ)，如空間站需要千米級尺寸的太陽能電池陣列來提供充裕的能量[1]；高分辨率對地觀測需要百米級的星載天線來提高精度[2-3]；深空探測飛行器需要用超輕薄膜構(gòu)成的巨型太陽帆來利用太陽光子作為動力[4-5]。但是受運(yùn)載火箭整流罩尺寸的限制，這類大型空間結(jié)構(gòu)無法由運(yùn)載器直接送入太空，需要由桁架在軌裝配而成[6-10]。

桁架在軌裝配過程中，空間機(jī)器人不可避免地需要利用視覺伺服完成大量對準(zhǔn)工作。傳統(tǒng)的機(jī)器人視覺伺服系統(tǒng)都是在對相機(jī)模型和機(jī)器人模型標(biāo)定的基礎(chǔ)上實(shí)現(xiàn)的，但在軌裝配過程中無法實(shí)現(xiàn)對系統(tǒng)的精確有效標(biāo)定，譬如美國用于在軌裝配的Sky-worker機(jī)器人[11]有一只手臂專門搭載相機(jī)。每次進(jìn)行裝配對準(zhǔn)時(shí)，該機(jī)械臂就會規(guī)劃運(yùn)動到相關(guān)位置完成“看”的動作。因此每次對準(zhǔn)操作手眼之間的關(guān)系都不確定，而重新標(biāo)定十分復(fù)雜難以實(shí)現(xiàn)且會導(dǎo)致裝配效率低下，繼續(xù)沿用原標(biāo)定結(jié)果則會產(chǎn)生很大誤差[12]。除此之外已標(biāo)定結(jié)果也會受空間惡劣環(huán)境的影響而發(fā)生退化進(jìn)而影響裝配精度，如相機(jī)標(biāo)定參數(shù)會受到高輻射的影響而退化。無標(biāo)定視覺伺服可在不預(yù)先對相機(jī)模型、機(jī)器人模型和手眼關(guān)系標(biāo)定的前提下，直接利用圖像上的系統(tǒng)誤差，設(shè)計(jì)控制器驅(qū)動機(jī)器人運(yùn)動，使系統(tǒng)收斂到一個(gè)容許的誤差域內(nèi)[13]。無標(biāo)定視覺伺服省去了繁瑣的標(biāo)定過程，在控制效率、應(yīng)用便利性及性能上均有較大的優(yōu)勢[14]。

此外，一方面考慮到機(jī)器人每次進(jìn)行桁架對準(zhǔn)操作時(shí)，所持桁架隨著裝配進(jìn)行其形狀質(zhì)量都會發(fā)生變化，可能是尚未裝配連接的簡單桁架，也可能是已部分搭建完成的大型桁架。因此在軌裝配視覺伺服系統(tǒng)控制對象的慣性參數(shù)不確定性。另一方面用于在軌裝配的機(jī)器人通常為帶有機(jī)械臂的獨(dú)立航天器。雖然這類航天器可以在進(jìn)入太空前完成系統(tǒng)參數(shù)的標(biāo)定，但隨著在軌裝配的進(jìn)行，航天器燃料會逐漸消耗、目標(biāo)桁架會被釋放，機(jī)器人系統(tǒng)的慣性參數(shù)會發(fā)生變化[15]。傳統(tǒng)的基于機(jī)器人與被操作對象模型的控制方法未將其考慮在內(nèi)，導(dǎo)致對準(zhǔn)精度不高甚至是對準(zhǔn)失敗的問題?；？刂茟{借其快速響應(yīng)、對參數(shù)變化及擾動不靈敏、無需系統(tǒng)在線辨識及物理實(shí)現(xiàn)簡單等優(yōu)點(diǎn)被廣泛應(yīng)用于參數(shù)未定的機(jī)器人控制領(lǐng)域[16]。對于在軌裝配的無標(biāo)定視覺伺服系統(tǒng)而言，最為直接的思路就是將滑模控制方法擴(kuò)展到無標(biāo)定視覺伺服控制中。但圖像特征點(diǎn)的深度信息以倒數(shù)形式非線性的呈現(xiàn)在圖像雅可比矩陣中，無法滿足滑?？刂浦袑ξ粗獏?shù)線性化表述的要求。

國內(nèi)外學(xué)者針對參數(shù)不定的無標(biāo)定視覺伺服展開了研究：Cheah等[17]研究了當(dāng)特征點(diǎn)的深度信息保持不變時(shí)，用于描述機(jī)器人關(guān)節(jié)空間與圖像空間的系統(tǒng)模型可以使用未知參數(shù)進(jìn)行線性化描述，這為自適應(yīng)控制用于無標(biāo)定動力學(xué)視覺伺服提供了有力的理論支撐；在此基礎(chǔ)上，Cheah等[18]最早對無標(biāo)定視覺伺服進(jìn)行了研究，在目標(biāo)深度信息緩慢變化時(shí)實(shí)現(xiàn)無標(biāo)定視覺伺服，但該方法雅可比矩陣自適應(yīng)估計(jì)通常需要假設(shè)深度信息為固定值或者是緩變值，以便于未知變量的線性參數(shù)化與自適應(yīng)律對這些變量進(jìn)行在線估計(jì)。Santamaria等[19]將相機(jī)焦距與深度信息的比值作為參數(shù)，并利用PnP(Perspective-n-Point)算法，對該比值進(jìn)行在線估計(jì)，在相機(jī)光軸位置與垂直度弱假設(shè)的條件下，該方法可以對雅可比進(jìn)行較好的近似，在無人機(jī)的垂直起降控制中體現(xiàn)出良好的性能。Cheah等[20]對具有運(yùn)動學(xué)和動力學(xué)不確定性的機(jī)器人設(shè)計(jì)了基于自適應(yīng)算法的混合力位控制器，實(shí)現(xiàn)對未定參數(shù)的在線估計(jì)，保證了機(jī)器人雅可比矩陣在一定范圍內(nèi)變化時(shí)系統(tǒng)仍然是穩(wěn)定的，但該方法需要經(jīng)過復(fù)雜的參數(shù)估計(jì)，耗時(shí)長，無法滿足大型空間結(jié)構(gòu)的快速裝配。Liu等[21]提出了深度雅可比矩陣，該深度雅可比矩陣單獨(dú)提取與深度信息相關(guān)的項(xiàng)作為一個(gè)比例項(xiàng)，矩陣中不包含深度信息，使得未知參數(shù)線性的出現(xiàn)在機(jī)器人閉環(huán)動力學(xué)系統(tǒng)中；Liang等[22]在此基礎(chǔ)上引入了自適應(yīng)控制方法，由于無法獲取深度參數(shù)的精確值，只能利用估計(jì)值進(jìn)行代替，但是此方法無法從理論上保證系統(tǒng)的漸進(jìn)性和穩(wěn)定性。

綜上所述，提出的機(jī)器人在軌裝配無標(biāo)定視覺伺服對準(zhǔn)方法，在傳統(tǒng)的無標(biāo)定視覺伺服基礎(chǔ)上，設(shè)計(jì)了深度估計(jì)器，滿足了滑?？刂茖ξ粗獏?shù)線性化要求，從而引入滑?？刂品椒▽?shí)現(xiàn)對機(jī)器人運(yùn)動狀態(tài)的控制，解決了機(jī)器人及負(fù)載慣性參數(shù)不定導(dǎo)致對準(zhǔn)精度低的問題，使系統(tǒng)能夠在復(fù)雜的太空環(huán)境下完成在軌裝配任務(wù)。

1 伺服系統(tǒng)原理

圖像雅可比矩陣描述機(jī)器人關(guān)節(jié)運(yùn)動與圖像特征變化關(guān)系，是研究無標(biāo)定視覺伺服對準(zhǔn)系統(tǒng)的基礎(chǔ)。而圖像雅可比矩陣存在高度非線性，從簡化的角度可以將其視為一系列局部范圍內(nèi)非線性關(guān)系的線性近似。圖像雅可比矩陣的在線辨識的準(zhǔn)確性將直接影響到控制系統(tǒng)性能的好壞。因此，本節(jié)將簡單介紹圖像雅可比矩陣的推導(dǎo)并選用卡爾曼濾波法完成在線辨識。

1.1 圖像雅可比矩陣推導(dǎo)

采用針孔相機(jī)模型來描述所研究相機(jī)。假設(shè)空間中任意一點(diǎn)P在相機(jī)坐標(biāo)系Tc中的坐標(biāo)為(x,y,z)，即P=[xyz]T。點(diǎn)P在圖像平面的投影在圖像坐標(biāo)系Ti下表示為Pi，即Pi=[uv0]T，則有

(1)

式中：λ為相機(jī)的焦距。定義相機(jī)相對于世界坐標(biāo)系Tg的線速度和角速度分別為v=[vxvyvz]T和ω=[ωxωyωz]T，則空間點(diǎn)P相對于相機(jī)坐標(biāo)系Tc的相對速度為

(2)

聯(lián)立式(1)和式(2)可得

(3)

式(3)反映了單個(gè)特征點(diǎn)圖像空間到操作空間的映射關(guān)系，設(shè)

Jfi=

(4)

式中：Jfi為單個(gè)特征點(diǎn)的圖像雅可比矩陣，其值與相機(jī)的有效焦點(diǎn)長度λ以及目標(biāo)深度信息z有關(guān)。則式(3)可以寫為

(5)

式中：fi是某一特征點(diǎn)的2維圖像特征矢量；r為相機(jī)在任務(wù)空間中的坐標(biāo)參數(shù)。

若擴(kuò)展到m個(gè)特征點(diǎn)可得

(6)

式中：Jf為狹義圖像雅可比矩陣，f1,f2,…,fm代表m個(gè)特征點(diǎn)的2維圖像特征矢量。

建立機(jī)器人運(yùn)動學(xué)方程，即

(7)

聯(lián)立式(6)和式(7)可得描述關(guān)節(jié)運(yùn)動與圖像特征變化關(guān)系為

(8)

式中：J為圖像雅可比矩陣；n為機(jī)器人自由度數(shù)。

1.2 圖像雅可比矩陣在線辨識

無標(biāo)定視覺伺服系統(tǒng)的好壞很大程度上取決于圖像雅可比矩陣在線辨識的準(zhǔn)確度。因?yàn)樵谡麄€(gè)任務(wù)空間中，圖像雅可比矩陣的變化是相當(dāng)明顯的，如果不能實(shí)現(xiàn)在線辨識，將無法保證圖像反饋控制器的性能，甚至使系統(tǒng)出現(xiàn)振蕩?？柭鼮V波方法是一種誤差方差最小的狀態(tài)估計(jì)方法，普遍應(yīng)用于實(shí)時(shí)狀態(tài)估計(jì)、系統(tǒng)參數(shù)辨識和目標(biāo)跟蹤領(lǐng)域。相比于其他方法，卡爾曼濾波對圖像雅可比的在線辨識具有更高的魯棒性和抗干擾能力，是目前常用的方法之一。

將式(6)離散化可得

f(k+1)≈f(k)+J(r(k))Δr(k)

(9)

式中：k為連續(xù)過程的離散點(diǎn)；Δr(k)為相鄰兩個(gè)離散時(shí)間點(diǎn)之間相機(jī)坐標(biāo)參數(shù)變化。

定義mn×1維觀測矢量

(10)

定義圖像雅可比矩陣的觀測矢量x(k)為系統(tǒng)狀態(tài)，機(jī)器人運(yùn)動引起的圖像特征變化為系統(tǒng)輸出，即y(k)=f(k+1)-f(k)。建立系統(tǒng)狀態(tài)方程

(11)

式中：C(k)狀態(tài)變量到觀測矢量的轉(zhuǎn)換矩陣；η(k)、v(k)分別為狀態(tài)噪聲和圖像觀察噪聲。

由此可建立基于卡爾曼濾波方法的圖像雅可比矩陣的在線辨識遞推方程

(12)

2 深度估計(jì)器設(shè)計(jì)

空間在軌裝配過程中，伺服對準(zhǔn)操作一般發(fā)生在粗對準(zhǔn)之后，此時(shí)目標(biāo)桁架與對應(yīng)的接口距離較近，接口特征所在平面與相機(jī)成像平面基本平行，但是接口特征的深度信息未知。傳統(tǒng)的深度信息估計(jì)方法有單目深度估計(jì)法和雙目深度估計(jì)法。其中，單目深度估計(jì)法算法復(fù)雜、耗時(shí)長、實(shí)時(shí)性差、易受環(huán)境影響、估計(jì)精度難以保證。而雙目深度估計(jì)法兩個(gè)相機(jī)的位置距離限制了深度數(shù)據(jù)的獲取，且機(jī)器人運(yùn)動引起的抖動等不穩(wěn)定因素使得兩個(gè)相機(jī)的對齊困難。因此本小節(jié)在圖像雅克比矩陣的基礎(chǔ)上，利用最小二乘法設(shè)計(jì)深度估計(jì)器，通過對機(jī)器人和圖像運(yùn)動的測量數(shù)據(jù)來在線估計(jì)目標(biāo)特征的深度值。所設(shè)計(jì)的深度估計(jì)器簡單易行，只與特征提取精度相關(guān)，不受安裝位置與機(jī)器人運(yùn)動的影響，且估計(jì)精度可以滿足對準(zhǔn)需求。

根據(jù)式(4)建立的單個(gè)特征點(diǎn)圖像空間到操作空間的映射關(guān)系，可得圖像雅可比矩陣的前三列與目標(biāo)特征點(diǎn)的深度值有關(guān)，重新整理可得

(13)

式中：Jt為相機(jī)平移運(yùn)動對圖像特征矢量的影響；Jw為相機(jī)旋轉(zhuǎn)運(yùn)動對圖像特征矢量的影響。

將式(13)重新整理為

(14)

式(14)的右側(cè)是觀察到的光流減去由于相機(jī)旋轉(zhuǎn)產(chǎn)生的光流。相減得到的剩余光流即為相機(jī)平移所導(dǎo)致的光流。將式(14)整理為緊湊的線性方程

Aθ=b

(15)

利用最小二乘法對上式中的目標(biāo)特征深度值求解得

(16)

基于式(16)利用對圖像雅可比矩陣的在線辨識結(jié)果可以實(shí)現(xiàn)對圖像特征點(diǎn)深度信息的估計(jì)。在已知深度信息的基礎(chǔ)上，則可以為引入滑?？刂品椒▽?shí)現(xiàn)對動力學(xué)參數(shù)和負(fù)載參數(shù)未定情況下機(jī)器人關(guān)節(jié)的自適應(yīng)控制提供基礎(chǔ)。

3 視覺伺服滑?？刂?/h2>

基于前面兩小節(jié)所研究內(nèi)容，根據(jù)特征點(diǎn)的期望特征矢量即可得到機(jī)器人運(yùn)動的期望關(guān)節(jié)角。從控制的角度來看，純運(yùn)動學(xué)的控制由于忽略了機(jī)器人的非線性動力學(xué)特性，很難保證機(jī)器人控制動力學(xué)性能與穩(wěn)定性，無法滿足柔順裝配的要求。此外考慮到在軌裝配過程中機(jī)器人與被操作物慣性參數(shù)不定的特點(diǎn)，傳統(tǒng)的控制方法未將其考慮在內(nèi)，可能導(dǎo)致對準(zhǔn)精度差甚至是對準(zhǔn)失敗的問題。而滑?？刂凭哂许憫?yīng)速度快、抗干擾能力強(qiáng)的特點(diǎn)，因此本小節(jié)設(shè)計(jì)視覺伺服的滑模控制器，能夠有效克服機(jī)器人運(yùn)動學(xué)、動力學(xué)參數(shù)不確定性和負(fù)載慣性參數(shù)不定帶來的干擾，保證機(jī)器人快速有效的完成伺服對準(zhǔn)。

在軌裝配機(jī)器人動力學(xué)模型

(17)

式中：H(q)為正定質(zhì)量慣性矩陣；C(q)為科氏力與離心力項(xiàng)；τ為機(jī)器人關(guān)節(jié)力矩；Fe為機(jī)器人進(jìn)行在軌裝配操作時(shí)末端輸出力。

當(dāng)不知道機(jī)器人的慣性參數(shù)以及末端輸出力的大小時(shí)，根據(jù)計(jì)算力矩法，取控制律為

(18)

(19)

即

(20)

(21)

定義

(22)

定義滑模控制的滑模面為

(23)

(24)

取

(25)

式中：d為待設(shè)計(jì)的向量，則

(26)

選取

(27)

式中：η為正定矩陣。則

(28)

由此可以證明，整個(gè)控制系統(tǒng)可以從任意初始狀態(tài)趨向滑模面，滿足滑?？蛇_(dá)性。

由式(18)和式(25)得無標(biāo)定視覺伺服滑?？刂坡蔀?/p>

(29)

4 無標(biāo)定視覺伺服系統(tǒng)

搭建在軌裝配機(jī)器人的無標(biāo)定閉環(huán)伺服控制系統(tǒng)：以桁架接口的期望圖像特征與圖像處理所獲得實(shí)際特征作為控制系統(tǒng)的輸入，利用深度估計(jì)器獲取當(dāng)前目標(biāo)特征點(diǎn)的深度信息，完成圖像雅可比矩陣的在線辨識。在此基礎(chǔ)上得到下一時(shí)刻機(jī)器人期望的關(guān)節(jié)運(yùn)動，將其輸入到視覺伺服系統(tǒng)滑?？刂破髦幸则?qū)動機(jī)器人運(yùn)動。搭載在機(jī)器人末端的相機(jī)再將得到的圖像信息反饋回伺服控制系統(tǒng)，與期望圖像特征作對比指導(dǎo)機(jī)器人完成桁架的跟蹤對準(zhǔn)。整個(gè)控制系統(tǒng)框圖如圖1所示。

圖1 無標(biāo)定伺服控制系統(tǒng)框圖

定義無標(biāo)定視覺伺服對準(zhǔn)系統(tǒng)誤差為

ef(t)=fd(t)-fg(t)

(30)

(31)

將控制量離散化，得到k時(shí)刻最優(yōu)控制量為

u(k)=Δr(k+1)=r(k+1)-r(k)

(32)

式中：r(k)為k時(shí)刻相機(jī)在任務(wù)空間內(nèi)的坐標(biāo)參數(shù)。

為消除系統(tǒng)跟蹤靜差，同時(shí)克服圖像處理時(shí)延對桁架跟蹤對準(zhǔn)性能的影響，引入PI控制策略

(33)

式中：cp、ci分別為PI控制器的比例和積分系數(shù)。

5 仿真校驗(yàn)

為了驗(yàn)證所提出的機(jī)器人在軌裝配無標(biāo)定視覺伺服對準(zhǔn)方法的正確性，利用MATLAB軟件對其進(jìn)行仿真驗(yàn)證。通過2組不同深度信息下特征點(diǎn)在圖像空間的運(yùn)動軌跡與所提方法的仿真結(jié)果作對比，驗(yàn)證深度估計(jì)器的有效性。接著，通過在機(jī)器人系統(tǒng)和被操作物慣性參數(shù)不定情況下對引入滑模控制與未引入滑?？刂谱鲗Ρ?，驗(yàn)證引入滑?？刂坪髮刂凭群涂刂菩阅艿奶嵘?。仿真所用為6自由度機(jī)器人，DH參數(shù)如表1所示。

表1 機(jī)器人DH參數(shù)

圖2 特征點(diǎn)投影變化情況

圖3 機(jī)器人姿態(tài)變化情況

下面將通過2組不同深度信息下特征點(diǎn)的運(yùn)動軌跡與所提方法作對比：圖4(a)為采用所設(shè)計(jì)的伺服對準(zhǔn)控制方法得到的接口特征點(diǎn)的運(yùn)動軌跡圖；圖4(b)和圖4(c)分別為假設(shè)深度信息z=1 m和z=10 m其他條件相同時(shí)得到的接口特征點(diǎn)的運(yùn)動軌跡圖。

圖4 不同情況下特征點(diǎn)在圖像空間的運(yùn)動軌跡

通過對比可以看出：采用所設(shè)計(jì)的伺服對準(zhǔn)控制方法得到的特征點(diǎn)運(yùn)動軌跡收斂速度快、運(yùn)動過程平滑、運(yùn)動距離最優(yōu)，特征點(diǎn)在圖像空間的運(yùn)動軌跡明顯優(yōu)于其他兩種情況。尤其是z=10 m時(shí)的運(yùn)動軌跡收斂速度很慢且不再是直線。這是因?yàn)樵撉闆r下圖像雅可比矩陣無法很好的近似相機(jī)運(yùn)動與圖像特征點(diǎn)運(yùn)動之間的關(guān)系，在相機(jī)指引下的機(jī)器人每步運(yùn)動誤差都較大，因此特征點(diǎn)的運(yùn)動軌跡形成了一個(gè)鋸齒狀的路徑。而z=1 m時(shí)的運(yùn)動軌跡明顯要比z=10 m時(shí)的好，這是因?yàn)橄啾扔趯?shí)際深度信息(z=1.3 m)z=1 m比z=10 m的誤差更小，更接近于實(shí)際深度，因此收斂性要更好。

圖5為各個(gè)接口特征點(diǎn)的實(shí)際深度信息與估計(jì)深度信息的變化曲線。從圖中可以看出，估計(jì)深度在4 s內(nèi)迅速上升，然后跟蹤到實(shí)際目標(biāo)的深度，之后就能在控制器收斂過程中準(zhǔn)確跟蹤真實(shí)深度。由于初始階段深度信息估計(jì)值與實(shí)際值相差較大，此時(shí)圖像雅可比矩陣不能很好的表示相機(jī)運(yùn)動與圖像特征點(diǎn)運(yùn)動之間的關(guān)系，從運(yùn)動軌跡圖可以看出特征點(diǎn)的運(yùn)動方向是有偏差的。

圖5 深度估計(jì)值與真實(shí)值對比

下面將通過在機(jī)器人系統(tǒng)和被操作物慣性參數(shù)不定情況下對引入滑模控制與未引入作對比： 圖6(a)為機(jī)器人與負(fù)載參數(shù)未定時(shí)采用引入滑?？刂品椒ǖ玫降奶卣鼽c(diǎn)運(yùn)動誤差圖；圖6(b)則為未引入滑模控制得到的特征點(diǎn)的運(yùn)動誤差圖。通過對比兩圖可以看出：引入滑?？刂坪筇卣鼽c(diǎn)的圖像誤差在5 s內(nèi)迅速趨于0，在8 s時(shí)所有誤差都穩(wěn)定在0附近。最終的特征點(diǎn)誤差數(shù)量級為10-8pixel。而傳統(tǒng)的控制方法會導(dǎo)致接口4個(gè)特征點(diǎn)產(chǎn)生較大偏差，峰值的誤差為127 pixel遠(yuǎn)大于所設(shè)計(jì)控制方法的峰值誤差。在8 s時(shí)各特征點(diǎn)基本可以實(shí)現(xiàn)穩(wěn)定跟蹤，但仍存在2 pixel誤差。得益于無標(biāo)定視覺伺服閉環(huán)特性，最終在10 s時(shí)各個(gè)特征點(diǎn)的誤差都趨于0，但相比于所設(shè)計(jì)的控制方法收斂速度要慢的多。由此可以證明引入的視覺伺服滑?？刂破髂軐?shí)現(xiàn)在軌裝配中對接口目標(biāo)的快速穩(wěn)定的對準(zhǔn)跟蹤。

圖6 引入滑模控制的誤差對比

6 結(jié) 論

1) 設(shè)計(jì)了目標(biāo)特征的深度估計(jì)器，使用對機(jī)器人和圖像運(yùn)動的測量數(shù)據(jù)在線估計(jì)目標(biāo)特征深度值。

2) 引入視覺伺服滑?？刂品椒ǎx取圖像特征作為輸入，各關(guān)節(jié)速度為輸出，通過閉環(huán)反饋調(diào)節(jié)，實(shí)現(xiàn)在軌裝配中對桁架的對準(zhǔn)跟蹤，并通過仿真實(shí)驗(yàn)對控制策略的可行性和有效性進(jìn)行了驗(yàn)證。

利用所提方法可以免去復(fù)雜的標(biāo)定過程，同時(shí)克服了機(jī)器人與被操作物慣性參數(shù)不確定性因素給控制系統(tǒng)帶來的影響，保證機(jī)器人快速有效的完成桁架的在軌裝配對準(zhǔn)。