劉安東，盧威威，俞 立

(浙江工業(yè)大學(xué) 信息工程學(xué)院，杭州 310023)

E-mail：lad@zjut.edu.cn

1 引 言

在大批量生產(chǎn)的需求背景下，機械臂在物流、食品、電子和醫(yī)療等領(lǐng)域得到了廣泛應(yīng)用.然而目前的機械臂仍以示教型機械臂為主，即機械臂運動的軌跡、速度、加速度等一系列變量由程序事先給定，工作場景過于單一.其主要原因在于機械臂對外部環(huán)境變化的適應(yīng)能力有限，無法像人一樣感知環(huán)境變化.視覺傳感器在不接觸目標(biāo)工件的情況下能通過視覺信息反饋指導(dǎo)機器人工作，利用視覺伺服技術(shù)可以有效地提高機械臂的環(huán)境適應(yīng)能力，擴大機械臂系統(tǒng)的使用范圍.因此，機械臂視覺伺服控制引起了國內(nèi)外學(xué)者的廣泛關(guān)注[1-4].

根據(jù)建模方法的不同，視覺伺服可分為基于圖像的視覺伺服(IBVS)[2]、基于位置的視覺伺服(PBVS)[3]以及混合視覺伺服(HVS)[4].IBVS需要實時計算圖像雅克比矩陣，計算負荷量較大，而HVS無法確保在伺服過程中目標(biāo)物始終位于相機的視野之內(nèi).相比之下，PBVS利用期望位姿與反饋位姿的差值作為視覺傳感器的輸入量，在三維空間中完成伺服控制任務(wù)，可避免奇異性和局部極小值等問題.因此，位姿估計和機械臂控制是基于位置的機械臂視覺伺服任務(wù)的兩大重要組成部分.在位姿估計方面，文獻[5]通過卡爾曼濾波方法識別和跟蹤特征點運動，從而實現(xiàn)了多相機系統(tǒng)的實時位姿估計.文獻[6-8]在考慮了冗余特征點信息丟失的影響下，分別提出了基于擴展卡爾曼濾波(EKF)以及無跡卡爾曼濾波(UKF)的位姿估計方法.文獻[9，10]在EKF的基礎(chǔ)上增加了迭代和自適應(yīng)環(huán)節(jié)，極大提高了估計精度以及系統(tǒng)魯棒性.但是這幾類方法均要求噪聲服從高斯分布，然而實際系統(tǒng)中通常是有界噪聲，難以獲得噪聲的統(tǒng)計特性.

機械臂控制方面，近十年已提出了許多有效的算法，如PID控制、自適應(yīng)控制、模糊控制、滑模控制、神經(jīng)網(wǎng)絡(luò)等[11-16].文獻[12]針對兩連桿機械臂實現(xiàn)了基于視覺反饋的PID位置跟蹤控制.但PID算法不適用于復(fù)雜系統(tǒng).文獻[13，14]在視覺伺服系統(tǒng)中利用自適應(yīng)控制算法，保證了機械臂視覺伺服系統(tǒng)的魯棒性以及穩(wěn)定性.然而自適應(yīng)算法難以選取合適的參數(shù).文獻[15]設(shè)計了基于PBVS的二階滑?？刂破鲗崿F(xiàn)7自由度冗余機械臂的位姿控制，但是滑?？刂圃诨Ｃ嫔蠒a(chǎn)生抖振現(xiàn)象.盡管上述算法在不同方面提高了機械臂的控制性能，但是無法有效處理機械臂的實際控制約束問題，例如控制輸入飽和、任務(wù)空間受限、速度受限等.

模型預(yù)測控制(MPC)由于能夠顯性地處理系統(tǒng)約束問題，受到了廣大研究者的青睞[17-19].其中文獻[18]提出了一種基于PBVS的自觸發(fā)非線性MPC方法，大大減少了視覺系統(tǒng)的測量次數(shù)，從而有效提升了處理效率.文獻[19]在eye-in-hand框架下采用MPC方法實現(xiàn)機械臂在空間中的自動抓取.但是MPC算法中優(yōu)化問題的計算負荷很大，導(dǎo)致伺服效率不高.iLQR/iLQG算法[20-22]是一種高效求解優(yōu)化問題的方法，在保證最優(yōu)解精度的同時簡化了MPC的求解.針對非完整性四輪車，文獻[20]在考慮控制輸入計算時延的前提下，設(shè)計了基于iLQG的MPC方法有效解決了有限時域最優(yōu)控制問題.文獻[21]將MPC方法與iLQR算法相結(jié)合，在類人機器人上實現(xiàn)了平面游泳以及單腿跳躍的實驗.然而，上述方法中代價函數(shù)均選擇為二次形式，若將二次代價函數(shù)用于非線性系統(tǒng)，則系統(tǒng)可能會陷入局部最優(yōu)狀態(tài)并存在線性化誤差.

基于以上討論，本文著重于解決基于位置的機械臂視覺伺服中的位姿估計和目標(biāo)跟蹤控制問題.首先在PBVS方案下建立了關(guān)節(jié)機械臂運動學(xué)模型.再根據(jù)相機投影模型進行特征點變換，并利用MHE方法求解代價函數(shù)優(yōu)化問題，從而實現(xiàn)有界噪聲約束下的機械臂位姿估計.然后提出了基于iLQR的MPC方法，利用動態(tài)規(guī)劃原理以及對偶梯度上升方法求解控制輸入約束下的預(yù)測控制優(yōu)化問題，得到最優(yōu)控制序列.為了克服估計誤差和線性化誤差引起的擾動問題，采用極點配置方法設(shè)計了ESO對擾動進行前饋補償，并對觀測誤差有界性和閉環(huán)系統(tǒng)穩(wěn)定性進行了證明.最后通過數(shù)值仿真與經(jīng)典PBVS控制方法進行對比，驗證了本文所提算法的有效性.

2 問題描述

考慮如圖1所示基于位置的視覺伺服系統(tǒng)，主要可分為位姿估計以及控制器設(shè)計兩部分.位姿估計部分是將相機特征點的運動轉(zhuǎn)化為三維空間中末端執(zhí)行器的運動，從而實現(xiàn)二維到三維空間中的信息轉(zhuǎn)換.控制器設(shè)計部分則是利用估計得到的位姿計算機械臂的控制變量，并驅(qū)動機械臂逐漸達到理想位姿以實現(xiàn)對象的定位或跟蹤.

圖1 PBVS系統(tǒng)Fig.1 PBVS system

關(guān)節(jié)機械臂運動學(xué)方程可寫為如下形式：

q·=J(q2)u

(1)

其中，q=[qxqyqzqφqθqψ]T為位姿向量，位姿向量包含位置向量q1=[qxqyqz]T以及姿態(tài)(歐拉角)向量q2=[qφqθqψ]T，qφ、qθ、qψ分別表示滾動角(Roll)，俯仰角(Pitch)以及偏航角(Yaw)，u=[vxvyvzwφwθwψ]T為速度矢量.J(q2)∈R6×6是用歐拉角定義的變換矩陣，即：

J(q2)=J1(q2)0
0J2(q2)

(2)

從而可將式(1)改寫為如下離散時間形式：

qk+1=f(qk，uk)=qk+T·J(qk)·uk

(3)

其中，T為采樣時間.

本文的目的是在針對基于位置的機械臂視覺伺服系統(tǒng)(3)，采用MHE方法估計機械臂3維位姿信息；根據(jù)當(dāng)前估計位姿與期望位姿之差，采用基于iLQR的MPC算法設(shè)計視覺伺服控制器；進一步利用ESO方法解決擾動問題，從而實現(xiàn)伺服控制并驅(qū)動機械臂達到期望位姿.

3 位姿估計

本節(jié)將采用MHE方法實現(xiàn)機械臂位姿估計.機械臂位姿包括機械臂末端和目標(biāo)物之間的相對位姿以及其相對速度：

xk=[qkq·k]

(4)

相機投影模型如圖2所示，其中涉及的坐標(biāo)系包括：圖像坐標(biāo)系Oi-(Xi，Yi)，像素坐標(biāo)系Opixel-(m，n)，相機坐標(biāo)系Oc-(Xc，Yc，Zc)，物體坐標(biāo)系Oob-(Xob，Yob，Zob)，Xi和Yi軸分別與XC和YC軸平行.

圖2 相機投影模型Fig.2 Camera projection model

為了估計姿態(tài)xk，本文選取4個特征點.根據(jù)圖2，以第j個特征點為例描述該點映射到像平面上的像素坐標(biāo)與狀態(tài)量之間的關(guān)系.該特征點在圖像坐標(biāo)系和像素坐標(biāo)系下的坐標(biāo)分別為(qix，j，qiy，j)和(mj，nj).則像素坐標(biāo)與圖像坐標(biāo)之間的關(guān)系為：

mj
nj
1=1/dx0m0
01/dyn0
001qix，j
qiy，j
1

(5)

其中，1/dx，1/dy分別表示像素點的長和寬，(m0，n0)為圖像坐標(biāo)系原點在像素坐標(biāo)下的坐標(biāo).

相機坐標(biāo)系與物體坐標(biāo)系之間關(guān)系為：

Pcj=cRobctob
01×31Pobj=cTobPobj

(6)

其中，Pobj=(qobx，j，qoby，j，qobz，j，1)T表示物體坐標(biāo)系中第j個特征點的齊次坐標(biāo)，Pcj=(qcx，j，qcy，j，qcz，j，1)T表示相機坐標(biāo)系中第j個特征點的齊次坐標(biāo)，cTob=cRobctob

01×31為物體坐標(biāo)系到相機坐標(biāo)系的變換矩陣，cRob為3×3旋轉(zhuǎn)矩陣，ctob表示物體坐標(biāo)系原點在相機坐標(biāo)系下的坐標(biāo).

由相機模型可得相機坐標(biāo)系與圖像坐標(biāo)系之間的投影關(guān)系為：

qix，j=Fqcx，jqcz，j，qiy，j=Fqcy，jqcz，j

(7)

綜合以上轉(zhuǎn)換關(guān)系，可得到像素坐標(biāo)系與物體坐標(biāo)系之間的關(guān)系為：

mjk
njk=fj(xk)=HccTobPobjGccTobPobj

(8)

其中，Hc=F/dx0m00

0F/dyn00表示相機內(nèi)參矩陣，F(xiàn)為焦距，Gc=[0 0 1 0].

令量測輸出yk=[m1k，n1k，…，m4k，n4k]T，非線性量測函數(shù)f(xk)=[f1(xk)，…，f4(xk)]T.由此建立系統(tǒng)狀態(tài)方程和量測方程如下：

xk+1=Cxk+ξk

(9)

yk=Dxk+ηk

(10)

其中，C=diag{a，a，a，a，a，a}為狀態(tài)矩陣，a=1T

01，ξk為k時刻的有界系統(tǒng)噪聲，ηk為k時刻的有界量測噪聲，D為與特征點像素坐標(biāo)(mjk，njk)T相關(guān)的量測矩陣，可通過f(xk)在平衡點x0附近泰勒展開得到.

根據(jù)滾動優(yōu)化策略，MHE的主要步驟可分為優(yōu)化，預(yù)測和傳輸3個部分：

優(yōu)化：根據(jù)代價函數(shù)進行優(yōu)化，從而估計xk-M在k時刻的估計值k-M，k，即：

k-M，k=arg minJMHE，k

(11)

其中，M為滾動時域窗口長度，代價函數(shù)定義如下：

JMHE，k=σ‖k-M，k-k-M‖2+∑ki=k-M‖yi-Di，k‖2

(12)

其中，σ為正常數(shù).

預(yù)測：根據(jù)預(yù)測方程(13)和估計值k-M，k得到預(yù)測值k-M+1為：

k-M+1=Ck-M，k，k=0，1，…

(13)

傳輸：k-M+1，k，…，k，k可通過估計值k-M，k來表示：

i+1，k=Ci，k，i=k-M，…，k-1

(14)

根據(jù)上述步驟，則位姿估計問題描述如下：

問題1.給定k-M時刻的預(yù)測值k-M和從k-M到k時刻的量測序列ykk-M，通過最小化代價函數(shù)(12)，可得系統(tǒng)(9)、(10)的最優(yōu)位姿估計值*k-M，k為：

*k-M，k=argmink-M，kJMHE，k
s.t.k-M，k∈X
-rξ≤ξk≤rξ
-rη≤ηk≤rη

(15)

預(yù)測更新方程為：

*k-M+1=C*k-M，k

(16)

注1.MHE是預(yù)測控制的對偶問題，其主要思想是利用有限個測量輸出來估計當(dāng)前系統(tǒng)狀態(tài).所以，MHE優(yōu)化問題本質(zhì)上與MPC是一致的，為此優(yōu)化問題(15)的求解過程在此省略，詳見第3節(jié)MPC的求解過程.

4 控制器設(shè)計

本節(jié)的目的是根據(jù)當(dāng)前估計位姿與期望位姿之差，設(shè)計視覺伺服控制器.由于PBVS系統(tǒng)可測量狀態(tài)為二維平面上的圖像像素坐標(biāo)，無法直接獲得笛卡爾空間三維坐標(biāo)，因此可根據(jù)最優(yōu)估計值*k，t中的位姿*k，t與期望位姿qd之差定義在k時刻的位姿誤差ek，t：

ek+1，t=*k+1，t-qd

(17)

由文獻[23]可知，滾動時域估計器所獲得的估計誤差是有界的，因此采用估計位姿來定義位姿誤差會引入一定的干擾.對式(3)在平衡點附近進行線性化(qk=qd，uk=0)并帶入式(17)可得：

ek+1，t=*k+1，t-qd
=*k，t+Buk，t+vk-qd
=ek，t+Buk，t+vk

(18)

其中，B=T·J(qd)，vk為估計誤差和系統(tǒng)線性化誤差引入的干擾且有界.為此，本文采用基于ESO的MPC方法解決具有外部擾動和約束的視覺伺服控制問題，控制結(jié)構(gòu)圖如圖3所示.

圖3 預(yù)測控制結(jié)構(gòu)圖Fig.3 Predictive control structure diagram

4.1 基于iLQR的MPC

為了解決具有約束的視覺伺服控制問題，首先定義如下的MPC的性能指標(biāo)：

JMPC，t=∑t+N-1k=tL(ek，t，uk，t)+L(eN)

(19)

其中，L(ek，t，uk)=‖ek，t‖2Q+‖uk，t‖2R為代價函數(shù)，L(eN)=‖et+N，t‖2W為終端代價函數(shù)，N是預(yù)測步長，Q，R是正定權(quán)重矩陣，W是需要設(shè)計的終端權(quán)重矩陣.

由于ESO采用前饋補償控制方法，預(yù)測控制器設(shè)計只需考慮名義系統(tǒng)如下：

k+1，t=k，t+Bk，t

(20)

則預(yù)測控制優(yōu)化問題描述如下：

問題2.在k時刻給定名義位姿誤差k，t，通過最小化MPC性能指標(biāo)，可得系統(tǒng)(20)的最優(yōu)名義控制輸入*k，t為：

*k，t=argmink，tJMPC，t
s.t.umin≤k，t≤umax

(21)

為了處理控制輸入約束，利用對偶梯度上升方法引入對偶變量ck和dk可得如下性能指標(biāo)：

J′MPC，t=∑t+N-1k=t[L(k，t，k，t)+ck(umin-k，t)+
dk(k，t-umax)]+L(N)

(22)

針對系統(tǒng)(20)，假定在t時刻存在一組軌跡：

{t，t，…，t+N-1，t，t+N-1，t，t+N，t}

(23)

使得優(yōu)化問題(21)存在最優(yōu)解，并由此根據(jù)最優(yōu)性原理設(shè)置值函數(shù)：

V(k，t)=minUtJ′MPC，t

(24)

其中，Ut={t，t+1，t，…，t+N-1，t}.

利用貝爾曼最優(yōu)性原理將式(24)整個控制序列的最小化改寫為單個控制信號最小化的迭代形式：

V(i，t)=mini，t[L(i，t，i，t)+V(g(i，t，i，t))]

(25)

其中，i∈[t，t+N-1].

根據(jù)式(25)定義ΔP為圍繞(i，t，i，t)的擾動函數(shù)：

ΔP(δi，t，δi，t)

=P(i，t+δ，i，t+δ，i)-P(i，t，i，t，i)

=L(i，t+δi，t，i，t+δi，t)-L(i，t，i，t)+

V(g(i，t+δi，t，i，t+δi，t))-V(g(i，t，i，t))

=12[1δi，tδi，t]0PTPT

PPP

PPP1

δi，t

δi，t

(26)

其中，泰勒系數(shù)P如下所示：

P=L+gTVi+1

P=L+gTVi+1

P=L+gTVi+1g+Vi+1g

P=L+gTVi+1g+Vi+1g

P=L+gTVi+1g+Vi+1g

利用一階KKT條件可得最小化式(26)的解析解δ*i，t為：

δ*i，t=arg minδiΔP(δi，t，δi，t)
=ri+Riδi，t

(27)

其中，ri=-P-1P，Ri=-P-1P.

將式(27)代入式(26)，可得：

ΔVi=12rTiPri+rTiP

(28)

Vi=P+RTiPri+RTiP+PTri

(29)

Vi=P+RTiPRi+RTiP+PTRi

(30)

通過計算ri，Ri，ΔVi，Vi以及Vi的值，從而完成向后遍歷步驟.然后利用向后遍歷步驟得到的序列{rt，Rt，…，rt+N-1，Rt+N-1}實現(xiàn)系統(tǒng)名義位姿誤差序列和名義控制序列的更新，完成向前遍歷.

t←t
i，t←i，t+ri+Riδi，t
i+1，t←i，t+Bi

(31)

可通過反復(fù)執(zhí)行向后遍歷以及向前遍歷步驟，得到最優(yōu)名義控制序列U*=[*t，…，*t+N-1，t]T，并利用文獻[24]中的拉格朗日乘子更新規(guī)則，更新式(22)中的ck和dk：

ck=ck+α(umin-k，t)
dk=dk+α(k，t-umax)

(32)

其中，α是更新率.

注2.由文獻[25]可知，iLQR算法是一種遞歸優(yōu)化算法，具有二次收斂特性，因此可以確保能得到式(27)中的最優(yōu)解δ*i，t.

4.2 擴張狀態(tài)觀測器

由式(17)可知，vk為估計誤差和系統(tǒng)線性化誤差引入的干擾且有界.不失一般性，假設(shè)誤差vk滿足以下增量形式：

vk+1=vk+Δvk

(33)

結(jié)合式(18)，可得到擴張狀態(tài)方程：

Xk+1=A1Xk+B1uk，t+S1Δvk

(34)

其中，Xk=ek，t

vk，A1=II

0I，B1=B

0，S1=0

I.為了對有界干擾進行估計，將Xk表示為式(34)的輸出信號，從而得到如下的觀測器方程：

k+1=A1k+B1uk，t+KobS2(Xk-k)

(35)

令Ek+1=Xk+1-k+1，并結(jié)合式(34)以及式(35)得到觀測誤差系統(tǒng)：

Ek+1=(A1-KobS2)Ek+S1Δvk

(36)

根據(jù)ESO方法，設(shè)計復(fù)合控制器：

uk，t=*k，t-B-1k

(37)

并將其代入式(18)，從而得到閉環(huán)系統(tǒng)模型：

ek+1，t=ek，t+B*k，t+vk-k

(38)

4.3 穩(wěn)定性分析

基于以上分析，本文將分別對觀測誤差模型有界性以及閉環(huán)系統(tǒng)模型穩(wěn)定性進行證明.首先，針對觀測誤差模型(36)可得以下定理.

定理1.若存在合適的觀測器增益矩陣Kob使得：

α=‖A1-KobS2‖<1

(39)

成立，則觀測誤差有界且滿足以下不等式：

‖Ek‖≤εk

(40)

序列εk是指數(shù)衰減的：

εk=αεk-1+β

(41)

其中，β=supk{‖S1Δvk‖}.

證明1.首先將算子應(yīng)用于觀測誤差方程(36)并結(jié)合三角不等式，可得：

‖Ek+1‖≤‖(A1-KobS2)Ek‖+‖S1Δvk‖
≤‖A1-KobS2‖·‖Ek‖+‖S1Δvk‖

(42)

由式(39)已知‖A1-KobS2‖<1，同時S1為確定的矩陣，vk為有界線性化誤差，因此‖S1Δvk‖必有界，則式(42)可改寫為如下形式：

‖Ek+1‖≤α‖Ek‖+β

(43)

根據(jù)式(41)以及式(43)可知，當(dāng)k→+∞時，在滿足α<1的條件下，εk指數(shù)衰減于漸近值ε∞=β1-α.因此結(jié)合式(40)可確保觀測誤差模型(36)是有界的.

然后針對閉環(huán)系統(tǒng)模型(38)的穩(wěn)定性結(jié)果證明可以表示如下.

定理2.針對性能指標(biāo)(19)以及閉環(huán)系統(tǒng)(38)，在給定權(quán)重矩陣Q和R的情況下，若存在合適的矩陣K以及終端權(quán)重矩陣W滿足以下不等式：

-W-10W-1+KTBTKTW-1
*-II00
**-W-100
***-R-10
****-Q-1≤0

(44)

則閉環(huán)系統(tǒng)(38)是穩(wěn)定的，同時J*MPC，t為李雅普諾夫函數(shù).

證明2.首先假定超過預(yù)測步長N的控制序列如下所示：

*k，t=Kek，t，k≥t+N

(45)

J*MPC，t=∑t+N-1k=t[‖ek，t‖2Q+‖*k，t‖2R]+‖et+N，t‖2W

(46)

根據(jù)最優(yōu)性原理可知，t+1時刻的性能指標(biāo)滿足下式：

J*MPC，t+1=∑t+Nk=t+1[‖ek，t+1‖2Q+‖*k，t+1‖2R]+‖et+N+1，t+1‖2W
≤∑t+Nk=t+1[‖ek，t‖2Q+‖*k，t‖2R]+‖et+N+1，t‖2W
=J*MPC，t+Φt-‖et‖2Q-‖*t‖2R

(47)

其中：

Φt=‖et+N+1，t‖2W-‖et+N，t‖2W+‖et+N，t‖2Q+‖*t+N，t‖2R
=‖(I+BK)et+N+1，t+vt-t‖2W-‖et+N，t‖2W+
‖et+N，t‖2Q+‖Ket+N，t‖2R

(48)

根據(jù)式(44)并通過舒爾定理可知Φt≤0，從而得到如下不等式：

J*MPC，t+1≤J*MPC，t-‖et‖2Q-‖*t‖2R
≤J*MPC，t

(49)

由式(49)可知，J*MPC，t為一李雅普諾夫函數(shù)，從而保證閉環(huán)系統(tǒng)穩(wěn)定.

證畢.

基于位置的視覺伺服控制算法如下所示.

算法1.

Step 1.在k=t時刻，利用MHE方法得到的估計位姿*k，t定義名義位姿誤差k，t，并隨機初始化名義控制序列uin=[k，…，k+N-1，k]T；

Step 2.利用uin以及k生成軌跡{k，k，…，k+N-1，k，k+N-1，k，k+N，k}；

Step 3.在j=1，2，…，g2情況下iLQR算法進行向后遍歷以及向前遍歷，當(dāng)j=g2時迭代結(jié)束，跳轉(zhuǎn)到Step 4；

Step 4.在i=1，2，…，g1情況下iLQR算法利用式(32)更新對偶變量，當(dāng)i=g1時迭代結(jié)束，跳轉(zhuǎn)到Step 5，否則返回Step 3；

Step 5.得到最優(yōu)名義控制序列U*=[*k，…，*k+N-1，k]T，則將U*中第一個值與ESO方法結(jié)合，通過設(shè)計合適的觀測器增益矩陣Kob確保α<1，從而對擾動vk進行估計；

Step 6.根據(jù)式(37)利用估計擾動k設(shè)計復(fù)合控制器，并完成機械臂控制；

Step 7.令k=k+1，返回Step 1繼續(xù)執(zhí)行.

5 仿真算例

為了驗證本文所提算法的有效性，本節(jié)考慮針對單關(guān)節(jié)機械臂利用MATLAB進行數(shù)值仿真算例，因此變換矩陣(2)可寫為如下形式：

J1(q2)=cqψcqθcqψsqθsqφ-sqψcqφcqψsqθcqφ+sqψsqφ

sqψcqθsqψsqθsqφ+cqψcqφsqψsqθcqφ-cqψsqφ

-sqφcqθsqφcqθcqφ

J2(q2)=1tqθsqφtqθcqφ

0cqφ-sqφ

0sqφ/cqθcqφ/cqθ

其中，s、c和t分別表示三角函數(shù)sin，cos以及tan.

仿真參數(shù)設(shè)置如下：

初始位姿參數(shù)q0=-0.50.10.3π5π4π3，期望位姿參數(shù)qd=[-1.20.550.9000]，預(yù)測步長N=10，采樣時間T=0.01s，控制輸入最大值以及最小值分別為：

umax=-umin=[0.4 0.8 0.5 0.6 0.7 0.5]

算法1中參數(shù)g1=2，g2=5，更新率α=1，正定權(quán)重矩陣Q和R分別為：

Q=diag(5，4.5，3，5，2，4)
R=diag(0.01，0.01，0.01，0.01，0.01，0.01)

終端權(quán)重矩陣W設(shè)計如下：

W=139.84 -5.57 -5.48-4.23 37.43 11.91
-5.57 34.55-27.97-22.44-22.25 18.30
-5.48-27.97 84.57-8.04-16.17 20.24
-4.23-22.44 -8.04114.91 131.79-59.96
37.43-22.25-16.17131.79 229.02-79.30
11.91 18.30 20.24-59.96-79.30 64.60

圖4為MHE方法的估計誤差曲線，其中圖4(a)為位置估計誤差，圖4(b)為姿態(tài)估計誤差.從中可以明顯看出，位置估計誤差在-0.06cm～0.05cm之間，姿態(tài)估計誤差則是保持在-0.05deg～0.05deg之間，MHE方法估計精度很高，因此可以將該位姿信息作為估計目標(biāo)的真實位姿.

圖4 MHE估計誤差Fig.4 MHE estimation error

圖5 ESO擾動估計Fig.5 ESO disturbance estimation

圖5為實際存在擾動以及ESO對于實際擾動的估計，其中圖5(a)為位置的擾動估計，圖5(b)為姿態(tài)的擾動估計.從圖5中可以明顯看出，位置擾動保持在0.1以內(nèi)，姿態(tài)擾動則是穩(wěn)定在0.15以內(nèi)，而ESO對于擾動的估計誤差范圍基本處于-0.01～0.01之間，估計效果非常好，有效解決了估計誤差和系統(tǒng)線性化誤差擾動問題，提高了系統(tǒng)的跟蹤性能.

圖6則為基于iLQR的MPC方法在預(yù)測步長N=10情況下的狀態(tài)軌跡變化曲線，其中圖6(a)為位置狀態(tài)軌跡，圖6(b)則為姿態(tài)狀態(tài)軌跡，并與經(jīng)典PBVS控制方法以及未加上擴張狀態(tài)觀測器的算法軌跡進行對比.首先將iLQR-MPC算法與經(jīng)典PBVS控制方法進行對比，可以明顯看出雖然兩者最后均收斂到了期望位姿qd附近，但是經(jīng)典PBVS控制方法基本是在15步左右才能夠達到收斂，而根據(jù)圖6中信息可知，iLQR-MPC算法在5步左右就達到收斂，控制效果良好，相比經(jīng)典PBVS控制方法所需時間更短.

圖6 狀態(tài)軌跡Fig.6 State trajectory

然而由圖6亦可知，iLQR-MPC算法存在擾動誤差問題，在位置軌跡方面，擾動誤差較小，x，y以及z方向上的誤差均處于0.02m～0.03m之間，然而在姿態(tài)軌跡方面，擾動誤差較大，Pitch誤差達到了0.04rad，而Roll以及Yaw的誤差更是達到0.09rad.從圖5中可以明顯看出，ESO具有良好的擾動估計精度，可以很好地消除擾動誤差的影響.因此相比單純的iLQR-MPC算法而言，加入ESO后可以減弱估計誤差以及線性化誤差的影響，從而大大減小了擾動誤差，狀態(tài)軌跡完全收斂到了期望位姿qd附近，誤差接近于0，控制效果良好.

圖7中顯示的則是算法在預(yù)測步長N=10情況下的控制輸入變化曲線，根據(jù)上文設(shè)置的仿真參數(shù)umax以及umin可以看出，控制輸入沒有發(fā)生超出約束條件的情況.

圖7 控制輸入Fig.7 Control input

6 結(jié) 論

本文著重于解決基于位置的機械臂視覺伺服中的位姿估計和目標(biāo)跟蹤控制問題.首先根據(jù)PBVS方案建立機械臂運動學(xué)模型，再基于相機投影模型利用MHE方法實現(xiàn)有界噪聲約束條件下的機械臂位姿估計，然后通過基于iLQR的MPC算法并結(jié)合對偶梯度上升方法處理控制輸入約束問題，后續(xù)利用ESO方法有效解決了估計誤差和系統(tǒng)線性化誤差引入的干擾問題.最后，仿真結(jié)果表明，本文所提算法相比經(jīng)典PBVS方法能夠更快達到期望位姿.