一種位置和圖像相切換的視覺伺服仿真分析

徐冰輝，張 磊，姚興田，蘇波泳

（南通大學(xué)機(jī)械工程學(xué)院，江蘇 南通 226019）

1 引言

智能化的時(shí)代，對(duì)機(jī)器人的需求也朝著智能化，多功能化的方向轉(zhuǎn)變，將視覺伺服技術(shù)加入機(jī)器人系統(tǒng)中能有效的提高機(jī)器人的自動(dòng)化程度，對(duì)擴(kuò)大機(jī)器人的應(yīng)用領(lǐng)域有著重要意義。

目前，機(jī)器人視覺伺服技術(shù)已廣泛運(yùn)用于產(chǎn)品分裝、貨物搬運(yùn)與裝配、機(jī)器人焊接［1］、噴漆［2］等領(lǐng)域。但實(shí)際應(yīng)用中仍然有許多關(guān)鍵問題需要解決，比如特征信息采集的穩(wěn)定性、機(jī)械手的位姿估計(jì)精度、機(jī)器人視覺伺服的局限性等。事實(shí)上，這些關(guān)鍵問題將直接影響機(jī)器人的伺服效果，因此，研究機(jī)器人視覺伺服技術(shù)對(duì)提高機(jī)器人抓取效果顯得十分重要。

視覺伺服的概念由文獻(xiàn)［3］在1979 年提出，首次將閉環(huán)系統(tǒng)引入視覺控制中，與傳統(tǒng)視覺反饋方式不同的是將圖像處理與反饋同時(shí)進(jìn)行，提高了控制精度。1980 年，Weiss 將視覺伺服區(qū)分為基于位置和基于圖像兩大類，之后出現(xiàn)的視覺伺服大多基于這兩大類。文獻(xiàn)［4］通過當(dāng)前圖像與目標(biāo)圖像之間的對(duì)極幾何關(guān)系獲取兩幅圖像間的位置方向參數(shù)。文獻(xiàn)［5］在此基礎(chǔ)上結(jié)合了神經(jīng)網(wǎng)絡(luò)控制，驅(qū)動(dòng)欠約束機(jī)器人運(yùn)動(dòng)到期望位置并完成抓取。而文獻(xiàn)［6］通過分解當(dāng)前圖像與目標(biāo)圖像之間的本質(zhì)矩陣計(jì)算得到旋轉(zhuǎn)和平移信息，同樣完成了伺服任務(wù)。文獻(xiàn)［7］增加了相機(jī)視野以及機(jī)械臂關(guān)節(jié)的限制的約束，研究出了基于勢場法的視覺伺服方法，有效的避免了圖像局部最小化的問題。文獻(xiàn)［8］將位置視覺伺服與PD 控制器結(jié)合，實(shí)現(xiàn)了對(duì)目標(biāo)的運(yùn)動(dòng)跟蹤。文獻(xiàn)［9］將CAD模型映射到圖像空間，使其收斂至目標(biāo)邊緣，實(shí)現(xiàn)了對(duì)物體的三維跟蹤。文獻(xiàn)［10］采用隨機(jī)搜索樹（RRT），提取出從初始位姿到目標(biāo)位姿的路徑，最終利用圖像伺服進(jìn)行跟蹤。

經(jīng)過多年的發(fā)展，大多數(shù)對(duì)視覺伺服的研究基本是基于位置視覺伺服或基于圖像視覺伺服的延伸，一些視覺伺服的本質(zhì)問題很難解決。如在位置視覺伺服中，伺服精度囿于機(jī)器人的運(yùn)動(dòng)學(xué)模型誤差；又比如在圖像視覺伺服中，只有當(dāng)機(jī)械手在接近期望位姿的區(qū)域內(nèi)才能保證伺服的穩(wěn)定性［11］。這里以特征點(diǎn)作為輸入信號(hào)，在Eye-to-Hand系統(tǒng)的基礎(chǔ)上結(jié)合基于圖像視覺伺服有著較好的魯棒性、局部穩(wěn)定性和收斂性［12］的優(yōu)點(diǎn)，以及基于位置的視覺伺服控制算法簡單、系統(tǒng)響應(yīng)快的優(yōu)點(diǎn)，設(shè)計(jì)了一種切換控制器將兩者優(yōu)勢相結(jié)合，提高機(jī)器人綜合視覺伺服效果。采用六自由度機(jī)器人，在伺服的前期選用位置伺服，使機(jī)械手能迅速到達(dá)目標(biāo)物的鄰域內(nèi)，然后在伺服的后期選用圖像伺服，不斷糾正機(jī)械手與期望位姿的偏差，使其最終到達(dá)指定期望位置。

2 機(jī)器人視覺伺服控制研究

2.1 位置視覺伺服原理

基于位置的視覺伺服中，圖像特征為目標(biāo)物的三維位姿。坐標(biāo)系模型，如圖1所示。在該視覺伺服中｛W｝表示機(jī)器人坐標(biāo)系同時(shí)也是世界坐標(biāo)系；｛E0｝表示機(jī)械手即機(jī)器人末端執(zhí)行器的初始坐標(biāo)系；｛E｝表示末端執(zhí)行器的當(dāng)前坐標(biāo)系；｛E*｝表示末端執(zhí)行器需要到達(dá)的期望坐標(biāo)系；｛C｝表示攝像機(jī)坐標(biāo)系。

圖1 坐標(biāo)系模型Fig.1 Coordinate System Model

基于位置視覺伺服的主要目的是輸出關(guān)節(jié)量Δθ＝［q1q2q3q4q5q6］T控制六自由度機(jī)器人使末端執(zhí)行器從初始位姿｛E0｝運(yùn)動(dòng)至期望位姿｛E*｝。采集到的特征點(diǎn)P通過PnP方法［13］可以計(jì)算出末端執(zhí)行器相對(duì)于攝像機(jī)的實(shí)時(shí)位姿，而末端執(zhí)行器期望位姿事先已給定為Tce*，根據(jù)位姿體系可以得出末端執(zhí)行器當(dāng)前位姿與期望位姿的偏差e為：

在末端執(zhí)行器從初始位姿移動(dòng)到期望位姿時(shí)，由于是Eyeto-Hand系統(tǒng)，即攝像機(jī)相對(duì)于機(jī)器人坐標(biāo)系的位姿Twc是固定的，并且可以通過手眼標(biāo)定獲得，最終可以較容易地得到末端執(zhí)行器相對(duì)于機(jī)器人坐標(biāo)系的位姿Twe為：

將位姿矩陣Twe0與Twe*代入由D-H法建立的機(jī)器人逆運(yùn)動(dòng)學(xué)模型可分別求解出機(jī)器人初始關(guān)節(jié)變量θ0和期望關(guān)節(jié)變量θ*，則該六自由度機(jī)器人需要輸出的關(guān)節(jié)量Δθ為：

在實(shí)際情況中，位姿的變換可能是相當(dāng)大的，上述運(yùn)動(dòng)往往不能一步到位，因此需要在每個(gè)采集周期更新一次位姿，最終不斷靠近期望位姿。由于上式（3）中用到的D-H 法建立在機(jī)器人各關(guān)節(jié)的機(jī)械尺寸上，涉及到的關(guān)節(jié)軸線交點(diǎn)、公垂線等參數(shù)都是難以確定的空間幾何數(shù)據(jù)。雖然通過視覺反饋可以彌補(bǔ)D-H建模帶來的定位誤差，但是在精度上仍然不盡如人意。并且零件的加工誤差、齒輪的嚙合間隙等都會(huì)產(chǎn)生系統(tǒng)誤差，因此最終的伺服結(jié)果很難等收斂。

2.2 圖像視覺伺服原理

基于圖像視覺伺服的圖像特征定義在圖像平面上，在伺服過程中攝像機(jī)實(shí)時(shí)采集機(jī)械手圖像，得到機(jī)械手的特征和期望特征的偏差作為偏差信號(hào)，通過圖像視覺伺服方法完成伺服任務(wù)。

這里定義的圖像偏差信號(hào)為：

式中：p*—期望的像素坐標(biāo)；p—當(dāng)前像素坐標(biāo)。

由像素平面特征點(diǎn)的速度可以計(jì)算出特征點(diǎn)在世界坐標(biāo)系下的速度υ。這里采用一個(gè)簡單的線性控制器：

式中：λ—增益系數(shù)。

得到在世界坐標(biāo)系下特征點(diǎn)的速度為：

式中：Jp—特征點(diǎn)的圖像雅克比矩陣。

由于圖像伺服最終驅(qū)動(dòng)的是機(jī)器人的關(guān)節(jié)，特征點(diǎn)在世界坐標(biāo)系下的速度最終還需轉(zhuǎn)為機(jī)器人的關(guān)節(jié)速度，而機(jī)器人雅克比矩陣就建立了關(guān)節(jié)速度與機(jī)器人末端手抓在世界坐標(biāo)系下的速度之間的數(shù)字關(guān)系。最終我們可以得到機(jī)器人的關(guān)節(jié)速度為：

式中：Jq-1—機(jī)器人雅克比矩陣的偽逆矩陣。

在采樣周期Tε的離散系統(tǒng)下可以得到機(jī)器人k時(shí)刻下關(guān)節(jié)變化量為：

由于基于圖像的視覺伺服方法不涉及機(jī)械手機(jī)械尺寸的建模，因此系統(tǒng)誤差較小，隨著機(jī)械手的位置不斷被伺服調(diào)整，最終伺服結(jié)果能達(dá)到收斂。

2.3 視覺伺服系統(tǒng)設(shè)計(jì)

這里將一個(gè)決策控制器和兩個(gè)視覺伺服控制器組合，形成簡單的切換系統(tǒng)。其中一個(gè)視覺控制器是基于位置視覺伺服，另一個(gè)是基于圖像視覺伺服，切換系統(tǒng)結(jié)構(gòu)，如圖2所示。

圖2 控制系統(tǒng)結(jié)構(gòu)圖Fig.2 Control System Structure

按照這里提出的視覺伺服控制方法，可以將該伺服過程分為兩個(gè)階段，（1）機(jī)械手離期望抓取位置較遠(yuǎn)時(shí)為第一階段，采用位置視覺伺服方法；（2）機(jī)械手靠近期望抓取位置時(shí)為第二階段，采用圖像視覺伺服方法。

切換示意圖，如圖3所示。圖中：｛E｝—當(dāng)前機(jī)械手坐標(biāo)系；｛E*｝—機(jī)械手最終到達(dá)的期望位姿坐標(biāo)系；d—機(jī)械手坐標(biāo)系原點(diǎn)與期望位姿坐標(biāo)系原點(diǎn)在世界坐標(biāo)系下的距離；ε—期望位姿的鄰域半徑。選用d作為切換信號(hào)，當(dāng)d大于給定的域值ε時(shí)選用基于位置的視覺控制器，當(dāng)d小于給定的域值時(shí)選用基于圖像的視覺控制器。切換控制偽代碼如下：

圖3 切換示意圖Fig.3 Switching Diagram

本研究使用Matlab/Simulink軟件，根據(jù)上述切換規(guī)則建立的機(jī)器人視覺伺服結(jié)構(gòu)圖，如圖4所示。

圖4 切換伺服系統(tǒng)結(jié)構(gòu)圖Fig.4 Switching Servo System Structure

2.4 探機(jī)器人視覺伺服模型建立

這里使用Matlab 軟件中的Robotics Toolbox 工具箱進(jìn)行建模，以puma560機(jī)器人為實(shí)例對(duì)象，以目標(biāo)物體上構(gòu)造的4個(gè)角點(diǎn)為特征點(diǎn)，在機(jī)械手末端執(zhí)行器坐標(biāo)系中的坐標(biāo)（單位：m）分別為（0.12，0.2，0）、（-0.12，0.2，0）、（0.12，-0.2，0）、（-0.12，-0.2，0）。攝像機(jī)焦距為10mm，幀率為25幀/秒，分辨率為512×512，像素的物理尺寸dx＝dy＝0.01mm，圖像平面的主點(diǎn)坐標(biāo)為（256，256），經(jīng)成像變換后特征點(diǎn)的初始像素坐標(biāo)為：（357，218）、（257，364）、（166，290）、（255，160），期望的像素坐標(biāo)為：（423，89）、（423，423）、（89，423）、（89，89）。為了模擬一個(gè)更加真實(shí)的環(huán)境，在機(jī)械手和攝像機(jī)的參數(shù)上添加了10%的建模誤差，所建立的機(jī)器人視覺伺服模型，如圖5所示。其中，世界坐標(biāo)系、攝像機(jī)坐標(biāo)系、物體坐標(biāo)系、機(jī)械手末端坐標(biāo)系分別為｛W｝、｛C｝、｛O｝、｛E｝，已知機(jī)械手末端的初始位姿Twe，攝像機(jī)相對(duì)于世界坐標(biāo)系下的位姿Twc，攝像機(jī)、機(jī)械手以及目標(biāo)特征點(diǎn)的初始位置如圖所示，其中‘*’表示目標(biāo)特征點(diǎn)。為了完成視覺伺服任務(wù)，機(jī)械手將帶動(dòng)目標(biāo)特征點(diǎn)一起運(yùn)動(dòng)至期望位置。

圖5 機(jī)器人視覺伺服仿真模型Fig.5 Robot Visual Servo Simulation Model

2.5 視覺伺服系統(tǒng)的仿真分析

在初始條件相同的情況下分別采用基于位置、基于圖像、和這里使用的視覺伺服方法對(duì)其仿真，其中對(duì)切換控制器仿真時(shí)，設(shè)置閾值ε為0.15。仿真結(jié)果，如圖6所示。圖中描述了機(jī)械手在移動(dòng)過程中，攝像機(jī)采集的特征點(diǎn)在圖像平面的運(yùn)動(dòng)軌跡，以及當(dāng)前像素坐標(biāo)與期望像素坐標(biāo)的誤差隨時(shí)間的變化。其中‘×’表示特征點(diǎn)的初始位置，‘o’表示特征點(diǎn)的期望位置，‘?’表示特征點(diǎn)移動(dòng)路徑。

圖6 三種視覺控制算法仿真結(jié)果Fig.6 Simulation Results of Three Visual Control Algorithms

由圖6（a）可知，特征點(diǎn)可以逐漸靠近期望位置，20s后伺服接近停止，但是不能達(dá)到期望位置，并且從圖6（b）可以發(fā)現(xiàn)圖像像素點(diǎn)的誤差最終并不能收斂于零，這與上述分析的位置視覺伺服方法最終的伺服結(jié)果很難收斂的結(jié)果相一致。由圖6（c）可知，基于圖像的視覺伺服可以使特征點(diǎn)移至期望位置。圖6（d）表明特征點(diǎn)的像素誤差隨著伺服時(shí)間的增加逐步收斂于零，運(yùn)行45s后伺服接近停止，說明該控制器對(duì)機(jī)械手運(yùn)動(dòng)學(xué)模型誤差和攝像機(jī)模型誤差并不敏感，能較高精度地完成視覺伺服任務(wù)，但相比位置伺服運(yùn)行時(shí)間較長。結(jié)合基于位置和圖像伺服的切換伺服策略后，由圖6（e）可看出前半程伺服運(yùn)動(dòng)與圖6（a）是相同的，這是因?yàn)樵谙到y(tǒng)沒有達(dá)到切換條件前選用的是基于位置視覺伺服，并且與位置視覺伺服相比可以發(fā)現(xiàn)特征點(diǎn)最終能夠達(dá)到期望位置，完成伺服任務(wù)。由于在前半程選用的是位置視覺伺服，而且通過上述分析可以知道位置視覺伺服的伺服時(shí)間短，因此使用該方法與基于圖像相比可以縮短一定的時(shí)間，從圖6（f）可以發(fā)現(xiàn)，整個(gè)視覺伺服過程在35s 左右圖像特征誤差收斂于0。通過計(jì)算可得，在基于位置視覺伺服中，機(jī)械手最終伺服停止的位置與期望位置的距離值d1＝0.1081m；在基于圖像視覺伺服中，機(jī)械手最終伺服停止的位置與期望位置的距離值d2＝0.002m。采用切換視覺伺服時(shí)，閾值ε選取不同值時(shí)，距離值d也不同，如圖7所示。

圖7 視覺伺服中不同閾值ε對(duì)應(yīng)的距離值dFig.7 Distance Value d Corresponding to Different Threshold ε in Visual Servo

由圖7可知，當(dāng)閾值ε<0.1時(shí)，機(jī)械手最終伺服停止的位置與期望位置的距離值d在0.1m左右，這是因?yàn)榍袚Q系統(tǒng)首先運(yùn)行的是位置視覺伺服，并且自始至終切換信號(hào)d>ε，無法滿足切換條件，最終如位置伺服一樣特征點(diǎn)無法達(dá)到期望位置，伺服失敗。

當(dāng)閾值ε>0.1時(shí)，切換信號(hào)d將在位置視覺伺服中一直減小到ε，此時(shí)觸發(fā)切換條件，然后在圖像視覺伺服中距離值d將一直減小，特征點(diǎn)到達(dá)期望位置，伺服成功。當(dāng)閾值ε分別取0.11以及0.09，其余條件與上述仿真初始條件相同，分別代入切換伺服控制器，當(dāng)前像素坐標(biāo)與期望像素坐標(biāo)的誤差隨時(shí)間的變化，如圖8所示。從圖8（a）可以發(fā)現(xiàn)系統(tǒng)達(dá)到切換條件，伺服成功；從圖8（b）可以發(fā)現(xiàn)當(dāng)閾值ε<0.1時(shí)，像素誤差變化如上圖8（b）一樣，說明系統(tǒng)一直采用基于位置的視覺伺服，沒有觸發(fā)切換條件，以上兩種仿真結(jié)果與本節(jié)對(duì)閾值ε范圍的分析完全相同。事實(shí)上，d1的值本質(zhì)上反映的是位置視覺伺服中機(jī)械手運(yùn)動(dòng)學(xué)模型誤差和攝像機(jī)模型誤差，而采用切換視覺伺服目的就是當(dāng)機(jī)械手即將到達(dá)位置視覺伺服最終的平衡位置時(shí)及時(shí)切換成圖像視覺伺服，從而減少機(jī)械手運(yùn)動(dòng)學(xué)模型誤差和攝像機(jī)模型誤差對(duì)視覺伺服的影響，使機(jī)械手快速準(zhǔn)確的到達(dá)指定位置。

圖8 基于切換伺服不同閾值下的特征像素誤差Fig.8 Characteristic Pixel Errors Based on Different Threshold of Switching Servo

3 結(jié)論

這里結(jié)合基于圖像伺服和基于位置伺服的優(yōu)點(diǎn)在機(jī)器人伺服系統(tǒng)中設(shè)計(jì)一種切換控制器。在伺服的前期選用位置伺服，使機(jī)械手能迅速到達(dá)目標(biāo)物的鄰域內(nèi)；在伺服的后期選用圖像伺服，不斷糾正機(jī)械手與期望位姿的偏差，使其最終到達(dá)指定期望位置，完成伺服任務(wù)。通過上述仿真實(shí)驗(yàn)可以發(fā)現(xiàn)，該切換視覺伺服充分的發(fā)揮了兩個(gè)子控制器的優(yōu)點(diǎn)，在前半程可以快速達(dá)到物體附近，后半程利用圖像伺服可以使機(jī)械手精確到達(dá)期望位置，最終較快、較準(zhǔn)地完成伺服任務(wù)。并且為了快速完成伺服任務(wù)，應(yīng)使機(jī)械手在位置視覺伺服中距離期望位置越近越好，然而在切換系統(tǒng)中設(shè)定的閾值并不是越小越好，還要考慮到位置伺服子控制器中機(jī)械手最終與期望位置的距離，否則達(dá)不到切換條件。