機器人在軌裝配無標定視覺伺服對準方法

賈慶軒，段嘉琪，陳鋼

北京郵電大學 自動化學院，北京 100876

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

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

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

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

綜上所述，提出的機器人在軌裝配無標定視覺伺服對準方法，在傳統(tǒng)的無標定視覺伺服基礎(chǔ)上，設(shè)計了深度估計器，滿足了滑模控制對未知參數(shù)線性化要求，從而引入滑模控制方法實現(xiàn)對機器人運動狀態(tài)的控制，解決了機器人及負載慣性參數(shù)不定導致對準精度低的問題，使系統(tǒng)能夠在復雜的太空環(huán)境下完成在軌裝配任務(wù)。

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

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

1.1 圖像雅可比矩陣推導

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

(1)

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

(2)

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

(3)

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

Jfi=

(4)

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

(5)

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

若擴展到m個特征點可得

(6)

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

建立機器人運動學方程，即

(7)

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

(8)

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

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

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

將式(6)離散化可得

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

(9)

式中：k為連續(xù)過程的離散點；Δr(k)為相鄰兩個離散時間點之間相機坐標參數(shù)變化。

定義mn×1維觀測矢量

(10)

定義圖像雅可比矩陣的觀測矢量x(k)為系統(tǒng)狀態(tài)，機器人運動引起的圖像特征變化為系統(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 深度估計器設(shè)計

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

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

(13)

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

將式(13)重新整理為

(14)

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

Aθ=b

(15)

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

(16)

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

3 視覺伺服滑模控制

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

在軌裝配機器人動力學模型

(17)

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

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

(18)

(19)

即

(20)

(21)

定義

(22)

定義滑模控制的滑模面為

(23)

(24)

取

(25)

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

(26)

選取

(27)

式中：η為正定矩陣。則

(28)

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

由式(18)和式(25)得無標定視覺伺服滑模控制律為

(29)

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

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

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

定義無標定視覺伺服對準系統(tǒng)誤差為

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

(30)

(31)

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

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

(32)

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

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

(33)

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

5 仿真校驗

為了驗證所提出的機器人在軌裝配無標定視覺伺服對準方法的正確性，利用MATLAB軟件對其進行仿真驗證。通過2組不同深度信息下特征點在圖像空間的運動軌跡與所提方法的仿真結(jié)果作對比，驗證深度估計器的有效性。接著，通過在機器人系統(tǒng)和被操作物慣性參數(shù)不定情況下對引入滑模控制與未引入滑模控制作對比，驗證引入滑模控制后對控制精度和控制性能的提升。仿真所用為6自由度機器人，DH參數(shù)如表1所示。

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

圖2 特征點投影變化情況

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

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

圖4 不同情況下特征點在圖像空間的運動軌跡

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

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

圖5 深度估計值與真實值對比

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

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

6 結(jié) 論

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

2) 引入視覺伺服滑模控制方法，選取圖像特征作為輸入，各關(guān)節(jié)速度為輸出，通過閉環(huán)反饋調(diào)節(jié)，實現(xiàn)在軌裝配中對桁架的對準跟蹤，并通過仿真實驗對控制策略的可行性和有效性進行了驗證。

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