徐冰輝,張 磊,姚興田,蘇波泳
(南通大學機械工程學院,江蘇 南通 226019)
智能化的時代,對機器人的需求也朝著智能化,多功能化的方向轉(zhuǎn)變,將視覺伺服技術加入機器人系統(tǒng)中能有效的提高機器人的自動化程度,對擴大機器人的應用領域有著重要意義。
目前,機器人視覺伺服技術已廣泛運用于產(chǎn)品分裝、貨物搬運與裝配、機器人焊接[1]、噴漆[2]等領域。但實際應用中仍然有許多關鍵問題需要解決,比如特征信息采集的穩(wěn)定性、機械手的位姿估計精度、機器人視覺伺服的局限性等。事實上,這些關鍵問題將直接影響機器人的伺服效果,因此,研究機器人視覺伺服技術對提高機器人抓取效果顯得十分重要。
視覺伺服的概念由文獻[3]在1979 年提出,首次將閉環(huán)系統(tǒng)引入視覺控制中,與傳統(tǒng)視覺反饋方式不同的是將圖像處理與反饋同時進行,提高了控制精度。1980 年,Weiss 將視覺伺服區(qū)分為基于位置和基于圖像兩大類,之后出現(xiàn)的視覺伺服大多基于這兩大類。文獻[4]通過當前圖像與目標圖像之間的對極幾何關系獲取兩幅圖像間的位置方向參數(shù)。文獻[5]在此基礎上結(jié)合了神經(jīng)網(wǎng)絡控制,驅(qū)動欠約束機器人運動到期望位置并完成抓取。而文獻[6]通過分解當前圖像與目標圖像之間的本質(zhì)矩陣計算得到旋轉(zhuǎn)和平移信息,同樣完成了伺服任務。文獻[7]增加了相機視野以及機械臂關節(jié)的限制的約束,研究出了基于勢場法的視覺伺服方法,有效的避免了圖像局部最小化的問題。文獻[8]將位置視覺伺服與PD 控制器結(jié)合,實現(xiàn)了對目標的運動跟蹤。文獻[9]將CAD模型映射到圖像空間,使其收斂至目標邊緣,實現(xiàn)了對物體的三維跟蹤。文獻[10]采用隨機搜索樹(RRT),提取出從初始位姿到目標位姿的路徑,最終利用圖像伺服進行跟蹤。
經(jīng)過多年的發(fā)展,大多數(shù)對視覺伺服的研究基本是基于位置視覺伺服或基于圖像視覺伺服的延伸,一些視覺伺服的本質(zhì)問題很難解決。如在位置視覺伺服中,伺服精度囿于機器人的運動學模型誤差;又比如在圖像視覺伺服中,只有當機械手在接近期望位姿的區(qū)域內(nèi)才能保證伺服的穩(wěn)定性[11]。這里以特征點作為輸入信號,在Eye-to-Hand系統(tǒng)的基礎上結(jié)合基于圖像視覺伺服有著較好的魯棒性、局部穩(wěn)定性和收斂性[12]的優(yōu)點,以及基于位置的視覺伺服控制算法簡單、系統(tǒng)響應快的優(yōu)點,設計了一種切換控制器將兩者優(yōu)勢相結(jié)合,提高機器人綜合視覺伺服效果。采用六自由度機器人,在伺服的前期選用位置伺服,使機械手能迅速到達目標物的鄰域內(nèi),然后在伺服的后期選用圖像伺服,不斷糾正機械手與期望位姿的偏差,使其最終到達指定期望位置。
基于位置的視覺伺服中,圖像特征為目標物的三維位姿。坐標系模型,如圖1所示。在該視覺伺服中{W}表示機器人坐標系同時也是世界坐標系;{E0}表示機械手即機器人末端執(zhí)行器的初始坐標系;{E}表示末端執(zhí)行器的當前坐標系;{E*}表示末端執(zhí)行器需要到達的期望坐標系;{C}表示攝像機坐標系。
圖1 坐標系模型Fig.1 Coordinate System Model
基于位置視覺伺服的主要目的是輸出關節(jié)量Δθ=[q1q2q3q4q5q6]T控制六自由度機器人使末端執(zhí)行器從初始位姿{E0}運動至期望位姿{E*}。采集到的特征點P通過PnP方法[13]可以計算出末端執(zhí)行器相對于攝像機的實時位姿,而末端執(zhí)行器期望位姿事先已給定為Tce*,根據(jù)位姿體系可以得出末端執(zhí)行器當前位姿與期望位姿的偏差e為:
在末端執(zhí)行器從初始位姿移動到期望位姿時,由于是Eyeto-Hand系統(tǒng),即攝像機相對于機器人坐標系的位姿Twc是固定的,并且可以通過手眼標定獲得,最終可以較容易地得到末端執(zhí)行器相對于機器人坐標系的位姿Twe為:
將位姿矩陣Twe0與Twe*代入由D-H法建立的機器人逆運動學模型可分別求解出機器人初始關節(jié)變量θ0和期望關節(jié)變量θ*,則該六自由度機器人需要輸出的關節(jié)量Δθ為:
在實際情況中,位姿的變換可能是相當大的,上述運動往往不能一步到位,因此需要在每個采集周期更新一次位姿,最終不斷靠近期望位姿。由于上式(3)中用到的D-H 法建立在機器人各關節(jié)的機械尺寸上,涉及到的關節(jié)軸線交點、公垂線等參數(shù)都是難以確定的空間幾何數(shù)據(jù)。雖然通過視覺反饋可以彌補D-H建模帶來的定位誤差,但是在精度上仍然不盡如人意。并且零件的加工誤差、齒輪的嚙合間隙等都會產(chǎn)生系統(tǒng)誤差,因此最終的伺服結(jié)果很難等收斂。
基于圖像視覺伺服的圖像特征定義在圖像平面上,在伺服過程中攝像機實時采集機械手圖像,得到機械手的特征和期望特征的偏差作為偏差信號,通過圖像視覺伺服方法完成伺服任務。
這里定義的圖像偏差信號為:
式中:p*—期望的像素坐標;p—當前像素坐標。
由像素平面特征點的速度可以計算出特征點在世界坐標系下的速度υ。這里采用一個簡單的線性控制器:
式中:λ—增益系數(shù)。
得到在世界坐標系下特征點的速度為:
式中:Jp—特征點的圖像雅克比矩陣。
由于圖像伺服最終驅(qū)動的是機器人的關節(jié),特征點在世界坐標系下的速度最終還需轉(zhuǎn)為機器人的關節(jié)速度,而機器人雅克比矩陣就建立了關節(jié)速度與機器人末端手抓在世界坐標系下的速度之間的數(shù)字關系。最終我們可以得到機器人的關節(jié)速度為:
式中:Jq-1—機器人雅克比矩陣的偽逆矩陣。
在采樣周期Tε的離散系統(tǒng)下可以得到機器人k時刻下關節(jié)變化量為:
由于基于圖像的視覺伺服方法不涉及機械手機械尺寸的建模,因此系統(tǒng)誤差較小,隨著機械手的位置不斷被伺服調(diào)整,最終伺服結(jié)果能達到收斂。
這里將一個決策控制器和兩個視覺伺服控制器組合,形成簡單的切換系統(tǒng)。其中一個視覺控制器是基于位置視覺伺服,另一個是基于圖像視覺伺服,切換系統(tǒng)結(jié)構,如圖2所示。
圖2 控制系統(tǒng)結(jié)構圖Fig.2 Control System Structure
按照這里提出的視覺伺服控制方法,可以將該伺服過程分為兩個階段,(1)機械手離期望抓取位置較遠時為第一階段,采用位置視覺伺服方法;(2)機械手靠近期望抓取位置時為第二階段,采用圖像視覺伺服方法。
切換示意圖,如圖3所示。圖中:{E}—當前機械手坐標系;{E*}—機械手最終到達的期望位姿坐標系;d—機械手坐標系原點與期望位姿坐標系原點在世界坐標系下的距離;ε—期望位姿的鄰域半徑。選用d作為切換信號,當d大于給定的域值ε時選用基于位置的視覺控制器,當d小于給定的域值時選用基于圖像的視覺控制器。切換控制偽代碼如下:
圖3 切換示意圖Fig.3 Switching Diagram
本研究使用Matlab/Simulink軟件,根據(jù)上述切換規(guī)則建立的機器人視覺伺服結(jié)構圖,如圖4所示。
圖4 切換伺服系統(tǒng)結(jié)構圖Fig.4 Switching Servo System Structure
這里使用Matlab 軟件中的Robotics Toolbox 工具箱進行建模,以puma560機器人為實例對象,以目標物體上構造的4個角點為特征點,在機械手末端執(zhí)行器坐標系中的坐標(單位:m)分別為(0.12,0.2,0)、(-0.12,0.2,0)、(0.12,-0.2,0)、(-0.12,-0.2,0)。攝像機焦距為10mm,幀率為25幀/秒,分辨率為512×512,像素的物理尺寸dx=dy=0.01mm,圖像平面的主點坐標為(256,256),經(jīng)成像變換后特征點的初始像素坐標為:(357,218)、(257,364)、(166,290)、(255,160),期望的像素坐標為:(423,89)、(423,423)、(89,423)、(89,89)。為了模擬一個更加真實的環(huán)境,在機械手和攝像機的參數(shù)上添加了10%的建模誤差,所建立的機器人視覺伺服模型,如圖5所示。其中,世界坐標系、攝像機坐標系、物體坐標系、機械手末端坐標系分別為{W}、{C}、{O}、{E},已知機械手末端的初始位姿Twe,攝像機相對于世界坐標系下的位姿Twc,攝像機、機械手以及目標特征點的初始位置如圖所示,其中‘*’表示目標特征點。為了完成視覺伺服任務,機械手將帶動目標特征點一起運動至期望位置。
圖5 機器人視覺伺服仿真模型Fig.5 Robot Visual Servo Simulation Model
在初始條件相同的情況下分別采用基于位置、基于圖像、和這里使用的視覺伺服方法對其仿真,其中對切換控制器仿真時,設置閾值ε為0.15。仿真結(jié)果,如圖6所示。圖中描述了機械手在移動過程中,攝像機采集的特征點在圖像平面的運動軌跡,以及當前像素坐標與期望像素坐標的誤差隨時間的變化。其中‘×’表示特征點的初始位置,‘o’表示特征點的期望位置,‘?’表示特征點移動路徑。
圖6 三種視覺控制算法仿真結(jié)果Fig.6 Simulation Results of Three Visual Control Algorithms
由圖6(a)可知,特征點可以逐漸靠近期望位置,20s后伺服接近停止,但是不能達到期望位置,并且從圖6(b)可以發(fā)現(xiàn)圖像像素點的誤差最終并不能收斂于零,這與上述分析的位置視覺伺服方法最終的伺服結(jié)果很難收斂的結(jié)果相一致。由圖6(c)可知,基于圖像的視覺伺服可以使特征點移至期望位置。圖6(d)表明特征點的像素誤差隨著伺服時間的增加逐步收斂于零,運行45s后伺服接近停止,說明該控制器對機械手運動學模型誤差和攝像機模型誤差并不敏感,能較高精度地完成視覺伺服任務,但相比位置伺服運行時間較長。結(jié)合基于位置和圖像伺服的切換伺服策略后,由圖6(e)可看出前半程伺服運動與圖6(a)是相同的,這是因為在系統(tǒng)沒有達到切換條件前選用的是基于位置視覺伺服,并且與位置視覺伺服相比可以發(fā)現(xiàn)特征點最終能夠達到期望位置,完成伺服任務。由于在前半程選用的是位置視覺伺服,而且通過上述分析可以知道位置視覺伺服的伺服時間短,因此使用該方法與基于圖像相比可以縮短一定的時間,從圖6(f)可以發(fā)現(xiàn),整個視覺伺服過程在35s 左右圖像特征誤差收斂于0。通過計算可得,在基于位置視覺伺服中,機械手最終伺服停止的位置與期望位置的距離值d1=0.1081m;在基于圖像視覺伺服中,機械手最終伺服停止的位置與期望位置的距離值d2=0.002m。采用切換視覺伺服時,閾值ε選取不同值時,距離值d也不同,如圖7所示。
圖7 視覺伺服中不同閾值ε對應的距離值dFig.7 Distance Value d Corresponding to Different Threshold ε in Visual Servo
由圖7可知,當閾值ε<0.1時,機械手最終伺服停止的位置與期望位置的距離值d在0.1m左右,這是因為切換系統(tǒng)首先運行的是位置視覺伺服,并且自始至終切換信號d>ε,無法滿足切換條件,最終如位置伺服一樣特征點無法達到期望位置,伺服失敗。
當閾值ε>0.1時,切換信號d將在位置視覺伺服中一直減小到ε,此時觸發(fā)切換條件,然后在圖像視覺伺服中距離值d將一直減小,特征點到達期望位置,伺服成功。當閾值ε分別取0.11以及0.09,其余條件與上述仿真初始條件相同,分別代入切換伺服控制器,當前像素坐標與期望像素坐標的誤差隨時間的變化,如圖8所示。從圖8(a)可以發(fā)現(xiàn)系統(tǒng)達到切換條件,伺服成功;從圖8(b)可以發(fā)現(xiàn)當閾值ε<0.1時,像素誤差變化如上圖8(b)一樣,說明系統(tǒng)一直采用基于位置的視覺伺服,沒有觸發(fā)切換條件,以上兩種仿真結(jié)果與本節(jié)對閾值ε范圍的分析完全相同。事實上,d1的值本質(zhì)上反映的是位置視覺伺服中機械手運動學模型誤差和攝像機模型誤差,而采用切換視覺伺服目的就是當機械手即將到達位置視覺伺服最終的平衡位置時及時切換成圖像視覺伺服,從而減少機械手運動學模型誤差和攝像機模型誤差對視覺伺服的影響,使機械手快速準確的到達指定位置。
圖8 基于切換伺服不同閾值下的特征像素誤差Fig.8 Characteristic Pixel Errors Based on Different Threshold of Switching Servo
這里結(jié)合基于圖像伺服和基于位置伺服的優(yōu)點在機器人伺服系統(tǒng)中設計一種切換控制器。在伺服的前期選用位置伺服,使機械手能迅速到達目標物的鄰域內(nèi);在伺服的后期選用圖像伺服,不斷糾正機械手與期望位姿的偏差,使其最終到達指定期望位置,完成伺服任務。通過上述仿真實驗可以發(fā)現(xiàn),該切換視覺伺服充分的發(fā)揮了兩個子控制器的優(yōu)點,在前半程可以快速達到物體附近,后半程利用圖像伺服可以使機械手精確到達期望位置,最終較快、較準地完成伺服任務。并且為了快速完成伺服任務,應使機械手在位置視覺伺服中距離期望位置越近越好,然而在切換系統(tǒng)中設定的閾值并不是越小越好,還要考慮到位置伺服子控制器中機械手最終與期望位置的距離,否則達不到切換條件。