未知雅可比建模的機器人視覺伺服自抗擾控制方法

仲訓杲，仲訓昱，彭俠夫，周承仙，徐 敏

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

傳統(tǒng)示教編程控制技術(shù)不能滿足機器人自主性要求，而機器人視覺伺服利用視覺傳感器獲取環(huán)境信息，對機器人實施反饋運動控制，已成為提高機器人自主操作能力的重要研究思路[1-2].當前機器人視覺伺服控制技術(shù)主要包括基于位置的視覺伺服(PBVS)和基于圖像的視覺伺服(IBVS)[3]. PBVS方法以笛卡爾坐標為控制量，結(jié)合已知目標幾何模型和相機標定參數(shù)，利用視覺測量原理對目標位姿進行估計，機器人根據(jù)目標姿態(tài)實施軌跡規(guī)劃與運動控制[4-5].與之相比，IBVS方法在視覺空間對圖像特征實施控制，把圖像誤差實時映射到機器人運動空間，實現(xiàn)機器人姿態(tài)協(xié)調(diào). IBVS方法因無需估計目標在笛卡爾空間中的3D位姿，簡化了系統(tǒng)計算，受到廣泛關(guān)注[6-7].

然而，基于IBVS的機器人控制方法面臨“視覺-運動”空間映射雅可比魯棒求解和圖像反饋控制器設計問題[8-9].其中求解雅可比要求系統(tǒng)標定參數(shù)和目標深度信息，導致精確雅可比計算困難，目前常用研究方法采用雅可比估計值替代精確值，例如Broyden雅可比估計法[10]、動態(tài)Broyden和動態(tài)指數(shù)加權(quán)遞歸最小二乘法(RLS)[11]、高斯-牛頓法和遞推RLS雅可比估計法[12].Hao等[13]把Broyden-Gauss-Newton、Broyden遞歸RLS統(tǒng)一到狀態(tài)空間中，研究基于狀態(tài)空間的視覺伺服方法. 這一類研究方案通過在線遞推得出雅可比估計值，但在控制器設計中不能完全保證雅可比可逆，機器人容易出現(xiàn)任務奇異.Liang等[14]研究了深度獨立圖像雅可比方法的視覺伺服.與之相似，Wang等[15]采用深度獨立圖像雅可比，建立圖像誤差與機器人關(guān)節(jié)之間的映射關(guān)系.王婷婷等[16]將特征點深度信息聯(lián)合像素坐標，研究模型預測控制(MPC)的IBVS方法.Gao等[17]研究動態(tài)自適應神經(jīng)網(wǎng)絡偽逆雅可比估計的視覺伺服方法. 以上研究方法解決深度獨立雅可比解耦控制和逆向雅可比求解問題，一定程度上改善了系統(tǒng)性能，有利于克服任務奇異的產(chǎn)生. 而本文研究思路與上述方法不同，將直接繞開雅可比復雜在線求解過程，機器人僅需在初始位姿引入試探運動，得出雅可比粗略值保證逆向雅可比存在，而在控制器設計中進一步對未建模雅可比實施反饋補償，提高機器人的自主性能.

綜上分析，機器人視覺伺服研究方法因需精確計算機器人雅可比，性能過于依賴系統(tǒng)標定參數(shù)和目標深度信息[18]，其標定參數(shù)變化與不確定性難免使現(xiàn)有研究方法應用受限.為此，本文研究一種未建模雅可比的機器人視覺伺服控制方法.研究方法借鑒無模型理論和非線性自抗擾控制器(active disturbance rejection controller，ADRC)構(gòu)建機器人“視覺-運動”空間反饋控制，其中采用跟蹤微分器(tracking differentiator，TD)跟蹤視覺空間期望特征，擴張狀態(tài)觀測器(extended state observer，ESO)實現(xiàn)“視覺-運動”空間未建模雅可比動態(tài)補償，最后利用非線性狀態(tài)誤差反饋(non-linear states error feedback，NLSEF)得出機器人運動空間控制量.

1 問題描述與方法分析

1.1 機器人視覺伺服

基于視覺伺服的機器人反饋控制，其控制器輸入量為視覺特征，輸出量為機器人運動量，控制目標是對視覺空間特征實施反饋控制，實現(xiàn)機器人運動空間任務操作.為此，定義視覺特征誤差es(t)?Rn，伺服控制器極小化以下代價函數(shù)：

(1)

若F(t)→0，則機器人從當前位姿收斂到期望操作位姿.

針對手眼標定六自由度機器人視覺伺服控制，假設機器人運動空間控制量為κR(t)∈R6，視覺空間n維圖像特征為κI(t)∈Rn.實際操作中，機器人末端輕微運動將導致視覺特征發(fā)生非線性變化，由此本文采用雅可比建立機器人“視覺-運動”空間微分關(guān)系，描述為：

κI(t)=Jκ(t)·κR(t).

(2)

上式雅可比Jκ(t)∈Rn×6可表示為

(3)

(4)

式中，λ為相機焦距長度,zi(t)為第i個特征點的深度信息，ui(t)、vi(t)為特征點i在圖像平面中的投影坐標. 從式(4)可看出構(gòu)建機器人視覺伺服控制首先需要介入系統(tǒng)標定和估算目標深度信息，從而求解式(3)雅可比映射矩陣，并在滿足微分方程(2)條件下設計有效的控制器，最終達到式(1)的控制性能.

1.2 ADRC控制思想

ADRC基于TD安排系統(tǒng)期望輸入，ESO估計系統(tǒng)未建模動態(tài)和外擾總作用，進而實施NLSEF控制，其原理框架如圖1所示.

圖1 ADRC原理框架Fig.1 The principle structure of the ADRC

假設被控對象為非線性n階不確定系統(tǒng)：

x(n)(t)=f(x(t),x′(t),…,x(n-1)(t))+b0u(t)+

w(t).

(5)

式中:w(t)為未知干擾，u(t)為控制量，b0為常量，系統(tǒng)擴展狀態(tài)向量為P(t)=[x1,x2…,xn+1]T=[x(t),…,x(n-1)(t),a(t)]T.其中a(t)=f(x(t),x′(t),…,x(n-1)(t))+w(t)，記為f(x(t),x′(t),…,x(n-1)(t))和w(t)在系統(tǒng)運行時的實時值.則ESO輸出值z2,i(t)對系統(tǒng)狀態(tài)變量實施跟蹤，即：

z2,i(t)→xi(t),i=1,2,…,n+1.

(6)

TD是信號處理環(huán)節(jié)，主要作用是安排系統(tǒng)參考輸入P*(t)微分信號的過渡，滿足：

(7)

式(6)中t→∞，TD輸出量z1,1(t)將跟蹤P*(t);而z1,j(t)，j=2,3,…,n跟蹤P*(t)的j階廣義微分信號.

此時，NLSEF將狀態(tài)誤差信號進行非線性配置，實現(xiàn)狀態(tài)誤差反饋，狀態(tài)誤差為：

el(t)=z1,l(t)-z2,l(t),l=1,2,…,n.

(8)

將誤差信號進行如下非線性NLSEF配置，得到系統(tǒng)控制：

(9)

式中bl、αl、δl為可調(diào)參數(shù)，fal(·)為非線性運算規(guī)則.

2 基于ADRC的機器人視覺伺服任務建模

假設機器人伺服系統(tǒng)采樣周期為h，則方程(2)可寫成離散ADRC控制器適用形式，有：

κI(k+1)-κI(k)=h·Jκ(k)·κR(k).

(10)

令視覺空間期望圖像特征κI*(k)∈Rn為伺服系統(tǒng)的參考輸入，設計跟蹤微分器TD安排輸入信號過渡，實現(xiàn)跟蹤κI*(k)，為此定義跟蹤誤差e1(k)∈Rn：

e1(k)=Z1(k)-κI*(k).

(11)

式中Z1(k)∈Rn為TD的輸出矢量，有

Z1(k+1)=Z1(k)-h·b1·fal(e1(k),α1,δ1).

(12)

上式非線性運算規(guī)則fal采用如下形式：

(13)

其中a、b、c為參數(shù)矢量.

考慮機器人系統(tǒng)誤差和外界干擾作用，進一步設計擴展狀態(tài)觀測器，定義ESO跟蹤誤差：

e2(k)=Z2(k)-κI(k),

(14)

式中Z2(k)∈Rn為ESO的輸出矢量，用于跟蹤當前時刻圖像特征κI(k)∈Rn，有

Z2(k+1)=Z2(k)+h·(Z3(k)-

b2·fal(e2(k),α2,δ2)+Jκ(k)κR(k)).

(15)

上式Z3(k)∈Rn表示系統(tǒng)未建模動態(tài)和外界干擾的總作用，給出：

Z3(k+1)=Z3(k)-h·b3·fal(e2(k),α3,δ3).

(16)

基于以上TD，ESO規(guī)劃，進一步設計機器人視覺控制器，首先給出機器人伺服系統(tǒng)反饋誤差：

e3(k)=Z1(k)-Z2(k).

(17)

此時采用NLSEF控制規(guī)則，可得機器人控制量κR(k)為：

Z3(k)),

(18)

式中，

κo(k)=h·b4·fal(e3(k),α4,δ4).

(19)

以上各式參數(shù)bi、αi、δi，i=1,2,…,4為常量，統(tǒng)稱為ADRC參數(shù).

3 雅可比未建模伺服控制器設計

機器人視覺伺服現(xiàn)實應用中，系統(tǒng)標定和雅可比求解困難，而現(xiàn)有雅可比估計方法影響機器人的控制性能.為此，本小節(jié)結(jié)合ADRC狀態(tài)反饋補償優(yōu)勢，構(gòu)建未建模雅可比的機器人實用視覺伺服控制方法.

不失一般性，假設目標特征點i在圖像平面位置為fi(t)=(ui(t),vi(t))，n個特征點組成圖像特征，記為視覺空間控制任務f(t)=[f1(t),f2(t),…,fn(t)]T.機器人運動空間控制量選取為U(t)=[V(t),W(t)]T，其中V(t)=(vx,vy,vz)為機器人末端線速度，W(t)=(wx,wy,wz)為角速度，根據(jù)式(2)建立微分方程：

f(t)=Jκ(t)·U(t).

(20)

考慮雅可比Jκ(t)估計值帶來的誤差為ζ(t)∈Rn，則式(20)改寫為

(21)

式中,

(22)

(23)

即

(24)

其中：

(25)

式(25)表示雅可比估計誤差和外界總擾動量.

圖2 基于ADRC的未知雅可比機器人視覺伺服控制框架Fig.2 The structure of ADRC-based unknown Jacobian robot visual servoing control

此時，根據(jù)上一小節(jié)ADRC控制器設計，以期望圖像特征f*(k)為系統(tǒng)參考輸入，TD實現(xiàn)對其跟蹤，對于機器人手眼抓取定位系統(tǒng)，期望圖像特征f*(k)為定值，因此TD模型相當于單位矩陣E.根據(jù)式(14)～(16)，二階ESO模型設計為：

(26)

由式(17)～(19)，NLSEF模型為：

(27)

機器人視覺伺服系統(tǒng)框架如圖2所示，可看出ADRC是對所有特征點的控制相互耦合在一起，完成機器人末端操作器定位控制.值得注意的是本文視覺伺服控制器的設計沿用了ADRC思想理論，其中TD、ESO、NLSEF的收斂性已被有效證明，具體可以參考文獻[19]. 而本文從工程應用角度出發(fā)，以機器人實際操作實驗證明提出方法的有效性，至于整個系統(tǒng)的穩(wěn)定收斂性則進行如下分析.

不失一般性，把式(27)代入式(21)，建立圖像運動與機器運動之間的系統(tǒng)方程，如下：

(28)

由式(27)，上式化簡可得：

f(t)=U0(t)+ε(t)-Z3(t)=h·b4·

fal(f*(k)-Z2(k),α4,δ4)+ε(t)-Z3(t).

(29)

如圖2所示，Z2(k)跟蹤當前時刻圖像特征f(t)，Z3(k)跟蹤未建模雅可比誤差和外界干擾總作用ε(t)，已知ESO收斂，那么有限時間內(nèi)Z2(t)→f(t)，Z3(t)→ε(t)，則式(29)寫成：

f(t)≈μ·fal(f(t)-f*(t),α4,δ4)，

(30)

此時控制器可以看著一階跟蹤微分器形式，選擇合理的參數(shù)，可以得出整個系統(tǒng)收斂穩(wěn)定.

4 實驗分析

如圖3所示，相機安裝在機器人末端，組成手眼標定視覺反饋實驗平臺.其中機器人選用UR5六自由度工業(yè)機械臂，機器人控制器作為下位機完成機器人運動學運算，同時驅(qū)動機器人各個關(guān)節(jié).上位機選用Intel Corei5 2.67 GHz CPU, 4 GB RAM計算機，用于圖像處理和運算視覺伺服控制算法.計算機與機器人控制器通過TCP/IP連接，構(gòu)成視覺伺服閉環(huán)系統(tǒng).

圖3 眼在手視覺伺服實驗平臺Fig.3 The eye-in-hand visual servoing platform

以機器人抓取定位為測試實驗，圖3(a)為機器人初始姿態(tài)及觀測圖像特征，圖3(b)為機器人抓取姿態(tài)及期望圖像特征，實驗設定控制率λ=0.25，系統(tǒng)采樣間隔為0.1 s，機器人控制量U(k)∈R6×1為末端線速度和角速度，為簡化特征提取過程，以4個圓點作為抓取定位特征，因此特征向量f(k)=[f1(k),f2(k),f3(k),f(k)4]T∈R8×1，式中fi=(uivi)為第i個特征點圖像坐標，在機器人初始姿態(tài)采用試探運動方法確定粗略雅可比為：

Jκ(0)=

實驗分別采用文獻[20]中基于卡爾曼濾波(Kalman filtering，KF)的雅可比估計IBVS控制策略、經(jīng)典PBVS控制策略[3]，以及本文研究的ADRC雅可比未建模視覺伺服控制策略進行對比，伺服任務為控制機器人末端從任意初始姿態(tài)定位到期望抓取姿態(tài)，并且抓取過程要求機器人大范圍運動.為了驗證各方法的魯棒性能，實驗設定特征點靠近圖像邊緣區(qū)域，因此初始特征為f(0)=(390.1,238.2,426.7,312.9,352.1,352.2,314.5,277.6)，期望特征為f*=(47.67,47.67,464.3,47.67,464.3,464.3,47.67,464.3)，3種方法實驗結(jié)果分析如下.

實驗1 采用本文基于ADRC雅可比未建模的視覺伺服控制策略.式(26)和(27)中，對其中一個特征點設置如下ADRC控制參數(shù)(參數(shù)采用經(jīng)驗法設定，其他特征點ADRC參數(shù)相同)：α2=α3=0.12，α4=0.53，δ2=δ3=0.5，δ4=1，b2=b3=1.43，b4=1.25.實驗結(jié)果如圖4所示：其中圖4(a)為圖像特征運動軌跡，圖中圓點代表期望特征位置，可以看出圖像空間特征軌跡平滑，且穩(wěn)定在相機的視場范圍內(nèi)，特征點沒有偏離視場；圖4(b)為機器人末端抓取定位運動軌跡，可知笛卡爾空間機器人接近以直線的運動方式從初始位姿定位到期望位姿，運動軌跡保持平滑穩(wěn)定，機器人各關(guān)節(jié)無沖突，并且沒有繞行回退現(xiàn)象發(fā)生；圖4(c)為圖像特征收斂誤差，圖4(d)為機器人末端運動速度，結(jié)果表明圖像特征收斂到期望特征，圖像誤差一致收斂趨向于0，同時機器人在笛卡爾空間位姿收斂，運動速度無震蕩，因此采用ADRC補償未建模雅可比視覺伺服控制效果良好.

實驗3 采用傳統(tǒng)PBVS的伺服控制方法.PBVS方法需要相機標定參數(shù)，參數(shù)包括圖像中心u0=v0=256，相機焦距λku=λkv=1 000，其中ku、kv分別為圖像u、v軸方向上的比例因子.測試結(jié)果如圖6所示：其中圖6(a)為圖像特征運動軌跡，可以看出圖像運動軌跡扭曲，特征點嚴重偏離相機視場范圍；圖6(b)為機器人末端抓取定位運動軌跡，由于PBVS方法直接對機器人運動實施控制，所以笛卡爾空間機器人運動軌跡以直線的方式定位到期望位姿，但是與4(b)相比，機器人運動在初始階段數(shù)據(jù)點密集，說明震蕩較大，而且直線運動存在關(guān)節(jié)受限的風險；圖6(c)為圖像特征收斂誤差，圖6(d)為機器人末端運動速度，結(jié)果可看出，與基于KF方法相似，收斂效果不及本文提出的視覺伺服控制方案.

圖4 本文提出的視覺伺服方法實驗結(jié)果Fig.4 The results of the proposed visual servoing method

圖5 基于KF雅可比估計的IBVS方法實驗結(jié)果Fig.5 The results of the KF-based Jacobian estimation IBVS method

圖6 傳統(tǒng)PBVS方法實驗結(jié)果Fig.6 The results of the traditional PBVS method

綜上實驗結(jié)果，與傳統(tǒng)KF和PBVS相比，本文采用基于ADRC補償?shù)难趴杀任唇Ｒ曈X伺服控制方法性能優(yōu)越，圖像特征運動軌跡平滑穩(wěn)定在相機視場范圍內(nèi)，且機器人末端運動軌跡平穩(wěn)無震蕩回退，機器人在視覺空間中的穩(wěn)態(tài)誤差在10個像素范圍內(nèi)，定位精度高.這主要原因有兩個：一方面ADRC降低了由雅可比計算精度帶來的系統(tǒng)內(nèi)擾；另一方面雅可比計算誤差通過ESO和NLSEF實現(xiàn)了非線性狀態(tài)反饋補償.本文采用的狀態(tài)誤差反饋補償方案是一種改善圖像特征運動軌跡和機器人末端運動軌跡、提高伺服控制系統(tǒng)性能的有效手段.

5 結(jié) 論

本文研究一種基于ADRC思想的機器人視覺伺服方法. 在未建模雅可比條件下，利用無模型理論和非線性自抗擾技術(shù)建立機器人“視覺間-運動”空間映射，算法通過TD跟蹤視覺空間期望特征，ESO實現(xiàn)未建模雅可比動態(tài)補償，最后利用NLSEF控制規(guī)則建立機器人伺服控制律. 本文構(gòu)建的視覺伺服控制方案，避免了機器人系統(tǒng)標定和雅可比在線求解難的問題. 最后在手眼標定六自由度機器人抓取定位比較的實驗結(jié)果表明，圖像空間特征軌跡與笛卡爾空間機器人末端運動軌跡平穩(wěn)，無震蕩回退，定位精度高. 研究方法實現(xiàn)了未知標定模型的機器人自主抓取定位操作.