基于圖像的非標定視覺反饋控制機器人全局定位方法

仲訓杲,徐 敏*,仲訓昱,彭俠夫

(1.廈門理工學院電氣工程與自動化學院,福建 廈門 361024;2.廈門大學航空航天學院,福建 廈門 361102)

隨著機器人應用的不斷推廣,機器人必將承擔多樣的任務,面臨多變的工作環境,而機器人利用視覺信息調整自身位形,由此構成的視覺伺服反饋控制方式,有利于提高機器人操作的靈活性和環境自適應性,在機器人控制中具有不可替代的作用[1].

根據視覺反饋信息的不同,機器人視覺伺服控制可分為基于位置的視覺伺服(position-based visual servoing,PBVS)和基于圖像的視覺伺服(image-based visual servoing,IBVS)[2].PBVS方法以目標3D笛卡爾坐標為反饋信號,由目標幾何模型及攝像機標定參數估計目標位姿,控制器根據機器人相對目標位姿進行軌跡規劃并實施接近目標控制.而IBVS方法直接以2D圖像為反饋信息,以當前圖像特征與期望特征之間的圖像誤差計算機器人控制量,并實施運動控制[3-5].相比于PBVS,IBVS因無需估計目標在笛卡爾空間中的3D位姿,精簡了三維重建而得到廣泛關注[6-8].然而,基于標定的經典IBVS控制方法依賴于圖像雅可比矩陣將圖像誤差函數映射到機器人運動中,該伺服方法一方面需要標定攝像機參數,另一方面需要特征點的深度信息[2],在非結構環境中難以發揮其優勢.為此,在非結構、非標定環境中,建立新的非標定IBVS控制方法成為當前的研究熱點.非標定IBVS的研究,其關鍵問題是在未知標定參數、深度信息條件下實時求解機器人“圖像空間-運動空間”之間非線性映射雅可比矩陣[9-14].

本文中針對機器人全局定位問題,研究一種基于圖像的非標定視覺伺服控制方法.首先采用反饋型Elman神經網絡(Elman neural network,ENN),對機器人“圖像空間-運動空間”之間非線性映射關系進行全局學習,得到機器人末端定位的次優狀態.在線測試階段,進一步采用卡Kalman濾波(Kalman filtering,KF)算法對ENN輸出狀態實施最優濾波,得到機器人定位的雅可比精確估計值.同時,KF狀態對ENN權重進行在線微調,保證ENN網絡下一時刻狀態輸出收斂穩定.該研究方法在線調節ENN網絡權值,保證機器人大范圍定位的穩定性,并使伺服系統具有一定的環境自適應性.此外,研究方法通過在線估計雅可比矩陣,不需要攝像機內部參數和目標深度信息,避免了攝像機和手眼標定.最后進行“眼在手”(eye-in-hand)六自由度機器人定位比較實驗,驗證了提出的非標定視覺伺服方法的可行性與優越性.

1 問題描述

以在板攝像機機器人視覺反饋為對象,視覺伺服系統以圖像特征為反饋信息,控制機器人從初始位形定位到期望位形.為此,在圖像平面中定義圖像誤差:

e(k)=S(k)-Sd,

(1)

式中，S(k)∈Rn×1為n維當前圖像特征向量,Sd∈Rn×1為n維期望特征向量,二者由攝像機投影模型得到.

攝像機投影模型如圖1所示,C{Oc-XcYcZc}為攝像機坐標系,I{OI-UV}為圖像平面坐標系.假設目標點在攝像機坐標系C中的笛卡爾坐標矩陣為P=[xc,yc,zc],攝像機坐標原點Oc與目標點P的連線與攝像機成像平面I相交一點S,則在小孔成像條件下,S即為P的成像點,S=[u,v]可表示為:

(2)

機器人視覺反饋控制通常以速度為控制量,因此通過計算機器人運動速度即可確定機器人位形.考慮六自由度機器人“眼在手”系統,并令機器人控制量

U(k)=[v(k),w(k)]T,

其中v(k)=[vx(k),vy(k),vz(k)],w(k)=[wx(k),wy(k),wz(k)]分別為機器人末端在基坐標系中的線速度和角速度,則機器人運動速度U(k)與圖像特征S(k)的變化量滿足以下映射關系[2]:

(3)

式中J(k)∈Rn×6為機器人位形與圖像特征的函數,定義為圖像雅可比矩陣.

例如,采用二維圖像點控制六自由度機器人運動,由攝像機投影模型(2)可知,若圖像無畸變,即圖像平面中U軸垂直V軸,則式(3)轉變為點特征圖像雅可比矩陣J′(k),有如下顯式:

J′(k)=

R2×6,

(4)

由式(4)可知,圖像雅可比矩陣包含了攝像機內部參數η和特征點深度信息zc,所以機器人位形對應的圖像特征與攝像機參數以及目標點深度信息相關,這意味著視覺反饋系統需要介入標定攝像機內部參數,即為機器人基于標定的經典IBVS工作方式.

而在攝像機參數和目標深度信息未知環境中實現機器人視覺反饋控制,其關鍵問題是圖像雅可比在線估計.本文把雅可比矩陣視為機器人“圖像空間-運動空間”非線性動態映射關系,并轉化為狀態估計問題.式(3)對應n維圖像特征向量雅可比矩陣不顯式表示為:

(5)

再把式(5)雅可比矩陣的6n個元素構建一個列向量X(k),形式如下:

(6)

式中jik代表雅可比J(k)的第i行和第k列元素.

不失一般性,把式(6)X(k)作為機器人伺服系統的狀態向量,并考慮離散動態系統:

X(k)=φ(k/(k-1))X(k-1)+F(k-1),

(7)

Z(k)=h(k)X(k)+V(k),

(8)

式中，φ(k/k-1)為狀態轉移矩陣,F(k)為過程噪聲,V(k)為觀測噪聲,方差分別為Q和R,Z(k)為觀測向量,根據式(3)定義:

Z(k)=S(k)-S(k-1)=J(k)U(k),

(9)

那么觀測矩陣h(k)可寫成:

(10)

KF算法作為最小方差狀態估計器,實用于離散系統的狀態估計,若系統(7)和(8)中過程噪聲F(k)和觀察噪聲V(k)為高斯白噪聲序列,那么應用KF算法對系統(7)和(8)進行狀態預測,即可得到雅可比估計值.然而,實際環境中,觀測噪聲V(k)為CCD傳感器的測量噪聲,不滿足白噪聲條件,再加上攝像機畸變和圖像處理算法精確性等因素,傳統KF效果不佳,在大范圍空間內由于遞推積累誤差的影響容易導致濾波器發散,從而機器人末端定位不準,甚至失敗.為此,本文中研究一種基于KF聯合反饋型ENN網絡的非標定全局空間機器人視覺伺服方法.

圖1 攝像機投影模型Fig.1 The projection model of camera

2 機器人全局空間雅可比映射學習

首先采用反饋ENN[15]對機器人“圖像空間-運動空間”非線性動態映射進行全局學習,得到伺服系統的次優狀態,使機器人末端能夠在大范圍內定位到期望位形的鄰域內.

反饋型ENN是一種典型的動態遞歸網絡,除了和普通神經網絡一樣包含隱含層和輸出層,還包含關聯層,所以ENN最大優點是關聯層具備一定的記憶功能,能夠記憶隱含層的激勵輸出,這使得ENN廣泛用于動態系統的辨識和預測控制等領域.

本文中采用的ENN網絡拓撲結構如圖2所示,網絡輸入為視覺特征向量S(k),輸出為伺服系統次優狀態向量X′(k),網絡數學模型描述如下:

輸入層輸入輸出關系式為:

OI(k)=f(Wl1S(k)+Wl2OC(k)-αi),

(11)

式中，OI(k)為輸入層的輸出量,S(k)為輸入圖像特征,OC(k)為關聯層的輸出量,Wl1、Wl2分別為輸入量和關聯層與輸入層之間的連接權值,αi為輸入層的偏置向量,f(x)為輸入層激勵函數,采用S型函數,如下式:

(12)

關聯層輸入輸出關系式為:

OC(k)=OI(k-1),

(13)

輸出層的輸入輸出關系式為:

X′(k)=Wl3OI(k)-αj,

(14)

式中，Wl3為輸入層與輸出層之間的連接權值,αj輸出層的偏置向量.

網絡各層連接權值Wli(i=1,2,3)通過離線訓練獲取.本文中選用四個圖像點組成八維特征向量,實現六自由度機器人運動控制,首先采用示教法控制機器人遍歷全局工作范圍,稀疏采樣600個樣本訓練ENN,輸入樣本為圖像特征集,即S=(S1(k),S2(k),…,S600(k))∈R8×600,其中Si(k)∈R8×1為第i個特征向量.輸出樣本為系統狀態向量集,即X=(X1(k),X2(k),…,X600(k))∈R48×600,其中Xi(k)∈R48×1為第i個特征向量對應的系統狀態.學習算法采用梯度下降法[15],網絡學習迭代110步,達到收斂穩定,訓練最小平方和誤差為1.3,說明網絡測試輸出大致接近訓練輸出,這意味著在全局空間范圍內,機器人末端能夠大致定位到期望位形的鄰域內.為實現機器人精確定位,下一步是對ENN輸出的次優狀態進行KF濾波,實現圖像雅可比矩陣在線精確估計.

圖2 反饋ENN網絡拓撲結構Fig.2 The structure of the ENN

3 非標定圖像視覺伺服控制框架設計

在非標定環境中,本研究將KF聯合反饋ENN,構建機器人全局狀態空間視覺伺服控制方案.框架如圖3所示,主要由控制律、KF聯合ENN雅可比在線估計、ENN權值更新組成.

圖3 非標定視覺伺服控制框架Fig.3 The structure of uncalibration visual servoing control

首先設計一個可靠的伺服控制律對整個機器人視覺伺服系統至關重要.式(1)期望特征Sd為已知定值,對式(1)求導可得:

(15)

考慮非零常數λ使以下等式成立:

(16)

將式(15)代入式(16)可得:

(17)

把式(17)代入式(3)可得:

-λe(k)=J(k)U(k),

(18)

上式變形得到伺服控制律:

U(k)=-λJ+(k)e(k),

(19)

式中λ為控制系數,J+(k)=J(k)T(J(k)J(k)T)-1為雅可比矩陣J(k)的廣義逆.

1) 首先視覺傳感器結合圖像處理算法提取圖像特征S(k),ENN網絡根據式(14)輸出系統次優狀態X′(k).

2) 令X(k-1)=X′(k),代入狀態轉移方程式(7)獲取伺服系統狀態更新值X(k).

3)X(k)帶入觀測方程(8)得到k時刻狀態觀測值Z(k).

初始化:J(0)∈R8×6;J(0)→X(0)∈R48×1

Q∈R48×48;R∈R8×8.

2)P(k/(k-1))=φ(k/(k-1))P(k-1)φ(k/(k-1))T+Q.

3)K(k)=P(k/(k-1))hT(k)(h(k)P(k/(k-1))h(k)T+R)-1.

5)P(k)=(E-K(k)h(k))P(k/(k-1))(E-

K(k)h(k))T+K(k)RK(k)T.

(20)

(21)

式中γ為學習率.

機器人非標定視覺伺服框架如圖3所示,基本步驟為,首先攝像機獲取圖像特征,ENN輸出系統次優狀態;在此基礎上利用KF算法實現最優狀態估計,得到雅可比精確估計值;其次ENN權值在線更新確保下一時刻系統穩定;最后機器人控制律以時間為指標,即在額定的采樣時間內評判圖像誤差,若達到額定采樣時間,則機器人定位結束,定位誤差值為像素.

4 結果比較分析

如圖4所示,采用微軟公司Kinect攝像機,并將攝像機固定在機器人末端,組成“眼在手”機器人實驗平臺.攝像機通過USB接口與個人PC相連,PC 通過 RS232 串口與機器人控制器相連接,構成機器人視覺伺服閉環系統.PC作為上位機主要完成圖像采集與圖像處理,并執行雅可比估計算法和伺服控制算法,機器人控制器作為下位機完成機器人運動學運算,同時驅動機器人各個關節.定位試驗以六自由度機器人末端線速度和角速度為控制量,即U(k)∈R6×1,以A4紙打印4個黑點為定位特征,那么特征向量S(k)為:

(22)

J(0)=

(23)

由式(6)可知,系統狀態向量X(k)大小為48×1,令狀態轉移矩陣φ(k)為單位矩陣,大小為48×48,控制率由實驗經驗選定為λ=0.15,采樣間隔為0.1 s.

圖4 “眼在手”機器人實驗平臺Fig.4 The robotic platform of "eye-in-hand"

為了驗證本文中非標定視覺伺服方法(簡稱本文方法)的有效性,以經典PBVS和IBVS方法[2]為比較對象,進行機器人大范圍定位試驗.試驗中PBVS和IBVS需要攝像機內部參數(本文中方法不需要攝像機參數),攝像機參數有：圖像中心u0=v0=256,攝像機焦距ηku=ηkv=1 000,ku、kv分別為圖像U、V軸方向上的比例因子.為了體現機器人大范圍定位,機器人初始位姿與期望位姿分別設定在機器人運動范圍的最大邊緣和最小邊緣處,對應初始圖像特征為S(0)=[378,235,429,301,363,354,311,289]T,期望圖像特征為Sd=[47,47,464,45,460,463,40,459]T.

PBVS方法試驗結果如圖5所示(圖中小方塊代表特征點期望位置),由圖5(a)可知,雖然PBVS方法機器人末端從初始位姿定位到期望位姿,機器人運動軌跡幾乎接近直線,震動小,但是圖5(b)圖像特征運動軌跡不佳,特征點容易超出攝像機視場范圍.

圖5 PBVS方法結果Fig.5 The results of PBVS method

IBVS方法試驗結果如圖6所示,圖6(a)機器人運動軌跡不穩定,機器人在起始段產生較大震動.由圖6(b)可知,雖然IBVS方法圖像特征軌跡以直線方式從初始位置收斂到期望位置,特征點保持在攝像機視場范圍內,

圖6 IBVS方法結果Fig.6 The results of IBVS method

本文方法試驗結果如圖7所示,由圖7(a)可知,本文方法機器人幾乎以直線方式從初始位姿定位到期望位姿,機器人運動軌跡穩定,震動小;同時由圖7(b)可知,圖像特征軌跡以近直線方式從初始位置收斂到期望位置,并且特征點保持在攝像機視場范圍內.圖8為機器人定位誤差,3種伺服方法圖像誤差收斂趨向于0,機器人定位成功.

圖7 本文方法結果Fig.7 The results of proposed method

以上比較試驗可知,PBVS方法機器人運動軌跡良好,但圖像特征軌跡容易偏離攝像機視場范圍;IBVS方法圖像特征軌跡良好,但機器人運動軌跡初始段不穩定,產生較大震動;而本文中方法機器人運動軌跡穩定無震動,圖像特征軌跡保持在攝像機視場范圍內,綜合性能較好.

圖8 圖像特征誤差Fig.8 The error of image features

傳統KF方法試驗結果如圖9所示,圖9(a)為3D笛卡爾空間機器人運動軌跡,圖9(b)為2D圖像平面特征點運動軌跡.由圖9(a)可知,在噪聲干擾影響下機器人末端出現較大幅度的繞行回退運動,進而導致圖像特征軌跡扭曲,特征點偏離攝像機視場范圍(見圖9(b)).

圖9 KF算法結果Fig.9 The results of KF

而對于相同干擾條件下的機器人定位任務,本文中KF聯合ENN方法試驗結果如圖10所示,圖10(a)為3D笛卡爾空間機器人運動軌跡,圖10(b)為2D圖像平面特征點運動軌跡.由圖10(a)可知,機器人運動軌跡穩定,無繞行回退現象產生;同時由圖10(b)可知,圖像特征軌跡平滑,特征點保持在攝像機視場范圍內.

圖10 KF聯合ENN結果Fig.10 The results of KF unite ENN

另外,由圖11可以看出,在額定的采樣時間內,本文中KF聯合ENN方法圖像誤差收斂速度較KF方法快.以上結果其主要原因是首先由ENN得到機器人全局定位的次優狀態,在這基礎上KF實施雅可比精確估計,并實時調整ENN網絡權重,這種聯合工作方式有利于保證機器人全局空間運動的穩定性.

圖11 圖像特征誤差Fig.11 The error of image features

綜上可知,在未知攝像機參數及目標深度信息條件下,與經典PBVS和IBVS方法相比,本文中研究的非標定圖像視覺伺服控制方法性能良好.在考慮外界噪聲干擾條件下,和傳統KF方法相比,本文KF聯合ENN方法改善了機器人運動的穩定性能,并具有一定的自適應能力.說明本文研究的KF聯合ENN網絡的圖像雅可比動態估計方法,及構建的非標定視覺伺服控制方案真實有效.

5 結 論

針對非標定六自由度機器人全局空間定位問題,研究了KF聯合反饋ENN網絡學習的非標定圖像視覺伺服控制方案.ENN網絡首先對機器人“圖像空間-運動空間”非線性映射關系進行全局學習,獲取機器人定位的次優狀態,進而采用KF進行最優狀態估計,解決圖像雅可比在線估計問題.KF同時對ENN權重進行實時微調,有利于保證機器人定位全局穩定,并對環境干擾具有一定的自適應性.最后本文方法與經典PBVS、IBVS以及傳統KF方法進行六自由度機器人定位比較試驗,實驗結果驗證了本文方法的有效性.

[1] 賈丙西,劉山,張凱祥,等.機器人視覺伺服研究進展:視覺系統與控制策略[J].自動化學報,2015,41(5):861-873.

[2] CHAUMETTE F,HUTCHINSON S.Visual servo control part I:basic approaches[J].IEEE Robotics & Automation Magazine,2006,4(13):82-90.

[3] 李海鵬,邢登鵬,張正濤,等.宏微結合的多機械手微裝配機器人系統[J].機器人,2015,37(1):35-42.

[4] 楊唐文,高立寧,阮秋琦,等.移動雙臂機械手系統協調操作的視覺伺服技術[J].控制理論與應用,2015,32(1):69-74.

[5] REDWAN D,NICOLAS A,YOUCEF M,et al.Dynamic visual servoing from sequential regions of interest acquisition[J].The International Journal of Robotics Research,2012,31(4):1-19.

[6] JOSIP M,MIRJANA B,MOJMIL C.Comparison of uncalibrated model-free visual servoing methods for small-amplitude movements:a simulation study[J].International Journal of Advanced Robotic Systems,2014,108(11):1-16.

[7] WANG H L.Adaptive visual tracking for robotic systems without image-space velocity measurement [J].Automatica,2015,55(4):294-301.

[8] MA Z,SU J B.Robust uncalibrated visual servoing control based on disturbance observer[J].ISA Transactions,2015,59(8):193-204.

[9] ASADA M,TANAKA T,HOSODA K.Visual tracking of unknown moving object by adaptive binocu1ar visual servoing[C]∥Proeeedings of IEEE International Conference on Multisensor Fusion and Integration for Intelligent Systems.Taipei:IEEE,1999:249-254.

[10] PIEPMEOER J A,LIPKIN H.Uncalibrated eye-in-hand visual servoing[J].The International Journal of Robotics Research,2003,22(10/11):805-819.

[11] HAO M,SUN Z.A universal state-space approach to uncalibrated model-free visual servoing[J].IEEE/ASME Trans on Mechatronics,2012,17(5):833-846.

[12] LIANG X W,WANG H S,CHEN W D,et al.Adaptive image-based trajectory tracking control of wheeled mobile robots with an uncalibrated fixed camera[J].IEEE Transactions on Control Systems Technology,2015,23(6):2266-2282.

[13] WANG H,YANG B,LIU Y,et al.Visual servoing of soft robot manipulator in constrained environments with an adaptive controller[J].IEEE/ASME Trans on Mechatronics,2017,22(1):41-50.

[14] GAO J,PROCTOR A,BRADLEY C.Adaptive neural network visual servo control for dynamic positioning of underwater vehicles[J].Neurocomputing,2015,167 (4):604-613.

[15] ELMAN J L.Finding structure in time [J].Cognitive Science,1990,14(2):179-211.