李靜， 黃友銳， 韓濤， 蘭世豪， 陳宏茂， 甘福寶

(安徽理工大學(xué) 電氣與信息工程學(xué)院，安徽 淮南 232001)

0 引言

作為煤礦安全生產(chǎn)重要巡檢設(shè)備的礦用智能巡檢機器人因具有較強的環(huán)境適應(yīng)力、自主巡檢力等優(yōu)勢在煤礦生產(chǎn)以及其他極端環(huán)境下得到了廣泛應(yīng)用[1]。利用礦用智能巡檢機器人可以減輕現(xiàn)場人員的工作壓力，節(jié)省人力成本，提高巡檢效率。礦用智能巡檢機器人在巡檢時需要獲取外部環(huán)境信息，由此來驅(qū)動其完成相關(guān)巡檢任務(wù)，而煤礦環(huán)境具有作業(yè)空間狹小、視覺環(huán)境差、溫度高的特點，因此，對礦用智能巡檢機器人如何更加準確獲取視覺信息，并高效執(zhí)行相應(yīng)巡檢任務(wù)提出了更高要求。視覺伺服控制技術(shù)是一種能夠精準獲取機器人工作環(huán)境信息，并通過一定的映射關(guān)系將視覺信息轉(zhuǎn)換到機械臂關(guān)節(jié)角的運動空間中，通過速度控制器驅(qū)動機器人運動的關(guān)鍵技術(shù)。良好的視覺伺服控制策略不僅可以保證智能巡檢機器人作業(yè)精度，還能使其在復(fù)雜環(huán)境下穩(wěn)定運行以完成巡檢任務(wù)。視覺伺服控制對于礦用智能巡檢機器人的控制具有重要作用。

傳統(tǒng)的基于圖像的視覺伺服(Image-Based Visual Servoing,IBVS)控制和基于位置的視覺伺服控制都需要依靠模型標定技術(shù)，這些模型主要涉及機器人模型和攝像機模型，視覺伺服系統(tǒng)的整體性能受模型標定的影響，要實現(xiàn)高精度的模型標定比較困難。因為不論是對相機內(nèi)外參數(shù)的標定還是對機器人運動學(xué)模型的標定都存在一定缺陷，比如系統(tǒng)結(jié)構(gòu)的變化、攝像機位置與焦距的變化以及高溫強輻射等都會使標定結(jié)果產(chǎn)生很大誤差，且標定成本較高，所以無標定視覺伺服技術(shù)備受矚目。

無標定視覺伺服是在沒有對相機模型和機器人模型進行標定的情況下，利用攝像機獲得的視覺信息形成閉環(huán)系統(tǒng)來驅(qū)動機器人運動，完成相關(guān)的視覺伺服任務(wù)。無標定視覺伺服技術(shù)的核心問題是計算末端執(zhí)行器的圖像特征變化率與空間速度之間的映射關(guān)系，這通常反映在圖像雅可比矩陣中。IBVS控制器利用圖像雅可比矩陣的逆或偽逆映射圖像特征誤差，生成控制信號?？祽c生等[2]提出了一種遞推最小二乘法估計圖像雅可比矩陣的偽逆矩陣，無需計算雅可比矩陣偽逆，計算較簡單。趙杰等[3]利用動態(tài)擬牛頓法估計圖像雅克比矩陣, 采用迭代最小二乘法提高系統(tǒng)的穩(wěn)定性。以上2種方法在雅可比矩陣估計中存在容易受環(huán)境噪聲干擾的風(fēng)險，為此，趙清杰等[4]針對非線性高斯系統(tǒng)提出了利用粒子濾波算法估計雅可比矩陣，并通過實驗驗證了該方法不僅能避免系統(tǒng)標定,而且對系統(tǒng)噪聲的類型沒有具體要求。徐鵬等[5]在非高斯環(huán)境下采用一種擴展H∞粒子濾波算法對圖像雅可比矩陣進行在線辨識,提高了機器人軌跡跟蹤精度，但該算法存在收斂性不高的缺陷。Wang Fasheng等[6]提出了無跡粒子濾波器在線估計雅可比矩陣值方法，該方法在目標跟蹤任務(wù)中具有準確可靠的性能，但對于多自由度的機器人性能不佳。梁喜鳳等[7]提出了一種基于自適應(yīng)無跡卡爾曼濾波器的拾取機械手伺服控制方法,該方法對擾動噪聲適應(yīng)性較強，但實時性不高。王新梅等[8]結(jié)合卡爾曼濾波中噪聲的數(shù)學(xué)特性, 構(gòu)建了魯棒卡爾曼濾波模型，實現(xiàn)了時延情況下圖像雅可比矩陣較為準確的估計。王洪斌等[9]針對卡爾曼濾波中狀態(tài)向量的速度分量進行再估計，提出了一種修正卡爾曼濾波器對目標物體遠程運動估計的算法，提高了估計精度，但存在穩(wěn)定性能差的缺點。近年來神經(jīng)網(wǎng)絡(luò)因具有高速并行分布式處理的特點被廣泛應(yīng)用，章曉峰等[10]利用BP神經(jīng)網(wǎng)絡(luò)確定了機器人基座坐標系和攝像機坐標系之間的非線性映射關(guān)系,通過分揀機器人系統(tǒng)實驗證明了該神經(jīng)網(wǎng)絡(luò)能夠提高手眼標定的精確度。Z.R.Tsai等[11]提出了一種帶有抽頭延遲的遞歸神經(jīng)網(wǎng)絡(luò)(RNN), 解決了視覺伺服系統(tǒng)的時變時滯控制問題,該算法收斂性較高，但同時存在穩(wěn)定性不高的問題。Chi Gaoxuan[12]和Gu Jinan 等[13]結(jié)合計算機視覺技術(shù)和模糊理論，提出了一種基于模糊神經(jīng)網(wǎng)絡(luò)的視覺伺服控制策略，利用模糊神經(jīng)網(wǎng)絡(luò)逼近目標圖像特征與機器人關(guān)節(jié)位置變化之間的映射關(guān)系，不僅降低了計算量，且提高了雅可比矩陣估計精度。F.Nadi等[14]利用人工神經(jīng)網(wǎng)絡(luò)對圖像雅可比矩陣的偽逆進行估計，建立了目標特征點視覺空間速度信息與機器人關(guān)節(jié)空間的映射關(guān)系，使伺服控制器能夠跟蹤運動目標，但該方法不能提高視覺伺服控制系統(tǒng)的穩(wěn)定性。M.Mitic等[15]提出了一種基于人工神經(jīng)網(wǎng)絡(luò)的智能機器人移動學(xué)習(xí)方法，分別在離線和在線階段建立了圖像空間與執(zhí)行器之間的映射關(guān)系。但上述方法求得的雅可比矩陣都不是最優(yōu)解，機械手在笛卡爾空間中的路徑也不是最優(yōu)和近似最優(yōu)的，在無標定視覺伺服控制精度與魯棒性上不能同時表現(xiàn)出良好的性能。

針對現(xiàn)有礦用智能巡檢機器人無標定視覺伺服控制中圖像雅可比矩陣估計值不準確、魯棒性差的問題，提出了一種具有長短期記憶(Long and Short Term Memory, LSTM)的卡爾曼濾波(Kalman Filtering，KF)算法(KFLSTM算法)，該算法使用LSTM彌補由KF算法產(chǎn)生的估計誤差，將濾波增益誤差、狀態(tài)估計向量誤差、觀測誤差用于LSTM的在線訓(xùn)練，利用訓(xùn)練后的LSTM模型對雅可比矩陣進行最優(yōu)估計，成功解決了KF算法對雅可比矩陣估計值不準確的問題?；贙FLSTM算法建立了無標定視覺伺服控制模型，并將KFLSTM算法應(yīng)用在六自由度機器人視覺伺服仿真實驗中。仿真結(jié)果表明，基于該算法的IBVS控制模型對噪聲具有魯棒性，視覺伺服控制精度高。

1 基于KF的圖像雅可比矩陣在線估計

1.1 圖像雅可比矩陣模型

基于圖像的無標定視覺伺服控制需要建立圖像特征空間與機械臂運動空間之間的映射關(guān)系，并盡量減少當前圖像特征與所需圖像特征之間的誤差。而基于圖像雅可比矩陣的方法是目前在無標定視覺伺服中常用的一類方法，該方法的主要特點就是使用圖像雅可比矩陣模型來描述機器人手眼映射關(guān)系。

定義圖像特征誤差為

e=S-S*

(1)

式中：e為當前圖像特征值與所需圖像特征值之間的誤差；S為當前圖像的特征值；S*為所需圖像的特征值。

圖像特征在IBVS系統(tǒng)中的變化率與關(guān)節(jié)角的變化率之間的映射關(guān)系為

(2)

機械臂關(guān)節(jié)角變化率與圖像雅可比矩陣之間的關(guān)系為

(3)

(4)

式中：qi(i=1,2,…,n)為機器人第i個關(guān)節(jié)角度，n為關(guān)節(jié)角個數(shù)；sj(j=1,2,…,m)為第j個圖像特征，m為圖像特征個數(shù)。

該矩陣反映了機器人末端執(zhí)行器運動空間到圖像特征空間的變換關(guān)系。因此，對雅可比矩陣的估計問題便成了無標定視覺伺服控制的關(guān)鍵問題，本文引入KF算法將圖像雅可比矩陣估計問題轉(zhuǎn)換為對系統(tǒng)的狀態(tài)觀測問題。

視覺空間-運動空間滿足非線性映射關(guān)系：

(5)

式中ve(t)為t時刻機器人末端執(zhí)行器速度。

1.2 KF算法分析

傳統(tǒng)的KF算法是一種獨立高斯白噪聲下的最優(yōu)線性狀態(tài)估計算法，可用于雅可比矩陣的在線估計，KF的狀態(tài)模型和觀測模型分別為

X(t)=Φ(t,t-1)X(t-1)+W(t)

(6)

Z(t)=H(t)X(t)+V(t)

(7)

式中：X(t)為系統(tǒng)的狀態(tài)向量，主要由雅可比矩陣的行向量組成；Φ(t,t-1)為狀態(tài)轉(zhuǎn)移矩陣，一般情況下取單位矩陣；W(t)為狀態(tài)噪聲矩陣；Z(t)為系統(tǒng)的觀測向量；H(t)為系統(tǒng)的觀測矩陣；V(t)為觀測噪聲矩陣，W(t)和V(t)都是均值為零、協(xié)方差分別為Q(t)和R(t)的白噪聲矩陣。

在無標定視覺伺服系統(tǒng)中，將系統(tǒng)的觀測向量Z(t)和觀測矩陣H(t)分別表示為

(8)

(9)

基于KF的狀態(tài)估計可分為2步進行：第1步為狀態(tài)向量和誤差協(xié)方差矩陣的預(yù)測過程；第2步為濾波增益、狀態(tài)向量和誤差協(xié)方差矩陣的更新過程。

預(yù)測過程公式為

(10)

P(t/t-1)=Φ(t,t-1)P(t-1)Φ(t,t-1)T+

Φ(t,t-1)Q(t-1)Φ(t,t-1)T

(11)

更新過程公式為

K(t)=P(t/t-1)H(t)T(H(t)P(t/t-1)×

H(t)T+R(t))-1

(12)

(13)

P(t)=(1-K(t)H(t))P(t/t-1)×

(1-K(t)H(t))T+K(t)R(t)K(t)T

(14)

由上述KF算法分析可知，KF是一種對高斯白噪聲下的線性系統(tǒng)的狀態(tài)變量進行最優(yōu)估計的有效方法。

2 改進KF算法

KF是在環(huán)境噪聲為高斯白噪聲下的最優(yōu)線性狀態(tài)估計，但是在機器人運動的真實環(huán)境中由機器人末端執(zhí)行器引入的噪聲并不是簡單的高斯白噪聲，這就導(dǎo)致了KF的估計變成了次優(yōu)估計。為了得到最優(yōu)估計，本文提出了 LSTM網(wǎng)絡(luò)誤差補償模型，實現(xiàn)了非線性濾波動態(tài)補償。

(15)

2.1 LSTM構(gòu)建

LSTM是一種改良的遞歸神經(jīng)網(wǎng)絡(luò)，它與傳統(tǒng)的遞歸神經(jīng)網(wǎng)絡(luò)相比能夠很好地降低梯度消失和梯度爆炸的風(fēng)險，并且能夠更好地處理長期依賴問題。LSTM的主要結(jié)構(gòu)是3個門單元，它們分別是輸入門、遺忘門和輸出門。輸入門表示要保存的信息或者待更新的信息，遺忘門決定丟棄哪些信息，輸出門決定當前神經(jīng)元細胞輸出的隱向量。在本文中，LSTM起到了精確的誤差估計器的作用，其任務(wù)是將期望狀態(tài)與估計狀態(tài)之間的誤差最小化。LSTM結(jié)構(gòu)如圖1所示。

圖1 LSTM結(jié)構(gòu)

LSTM在實現(xiàn)信息傳遞的過程中首先是將當前單元的狀態(tài)輸入和前一個單元的隱藏層向量經(jīng)過遺忘門輸出一個遺忘向量，遺忘門將刪除不相關(guān)的信息。該過程的實現(xiàn)方程為

(16)

當前單元的狀態(tài)輸入和前一個單元的隱藏層向量會通過tanh函數(shù)計算當前單元的候選狀態(tài)，這一層將保存可能會加入細胞狀態(tài)的信息，并通過一個輸入門來決定有哪些信息要更新到網(wǎng)絡(luò)的長期記憶中。該過程的實現(xiàn)方程為

(17)

(18)

(19)

當前單元細胞的長期記憶會通過一個輸出門結(jié)構(gòu)得到當前輸出信息和當前隱藏層信息。該過程的實現(xiàn)方程為

(20)

(21)

2.2 神經(jīng)網(wǎng)絡(luò)誤差補償模型

圖2 基于LSTM的誤差補償模型

改進KF圖像雅可比矩陣在線估計算法步驟如下：

步驟2：獲取當前圖像特征的觀測值Z(t)。

步驟3：根據(jù)式(10)、式(11)，由預(yù)測步驟估計下一時刻的狀態(tài)值和協(xié)方差矩陣。

3 基于改進KF算法的無標定視覺伺服模型

為了在無標定視覺伺服控制中獲得更準確的雅可比矩陣估計值，采用了基于LSTM補償?shù)腒F算法(KFLSTM算法)，通過該算法獲得雅可比矩陣最優(yōu)值，進而得到雅可比矩陣廣義逆與特征誤差矩陣的乘積，使控制器輸出準確的關(guān)節(jié)角速度，從而控制機械臂的實時運行，提高視覺伺服控制的精度和收斂速度。基于KFLSTM算法的無標定視覺伺服控制模型框架如圖3所示。首先用傳統(tǒng)的KF算法估計圖像雅可比矩陣，得到KF的濾波增益誤差、狀態(tài)估計誤差和觀測誤差，將3個誤差向量構(gòu)造為單個向量，作為LSTM網(wǎng)絡(luò)的輸入，利用KF在每個過程中得到的雅可比矩陣和期望矩陣的差作為網(wǎng)絡(luò)的輸出，得到一組訓(xùn)練樣本，這些訓(xùn)練樣本用于訓(xùn)練LSTM神經(jīng)網(wǎng)絡(luò)，以確定網(wǎng)絡(luò)參數(shù)。

圖3 基于KFLSTM算法的無標定視覺伺服控制模型框架

基于KFLSTM算法的無標定視覺伺服控制模型工作流程如下：

[ds1,ds2,…,dsk][dq1,dq2,…,dqk]-1

(22)

Step5:由控制規(guī)則獲得機械臂關(guān)節(jié)速度, 控制機器人從當前位姿運動到下一時刻位姿，利用攝像機獲取圖像特征，若此時圖像特征誤差在收斂閾值范圍內(nèi),則結(jié)束IBVS循環(huán)，否則繼續(xù)用KFLSTM模型進行下一時刻(t+1)的圖像雅可比矩陣估計。

4 實驗結(jié)果與分析

4.1 實驗條件與參數(shù)設(shè)置

本文對基于KFLSTM算法的無標定視覺伺服控制模型的可行性進行了仿真實驗，并與經(jīng)典的基于KF算法和已經(jīng)取得較好性能的KFRBF(基于徑向基函數(shù)的卡爾曼濾波)算法進行比較。仿真實驗在Matlab環(huán)境下進行。仿真均以PUMA560機械臂為仿真模型，同時利用機器視覺工具箱搭建了攝像機模型，其中攝像機模型參數(shù)設(shè)置如下：焦距為8 mm，相位平面為1 024×1 024，主點坐標為(512,512)，采樣頻率為20 Hz。

在仿真實驗中，選取某一物體上的4個點即A，B，C，D的空間坐標作為無標定視覺伺服控制模型的目標特征點，且特征點在笛卡爾坐標系中的坐標為P，初始圖像特征為S0，期望圖像特征為S*，機械臂的初始角度為q0。圖像特征誤差收斂閾值為ethr=0.5。

(23)

設(shè)置初始圖像特征點像素位置為

S0=[xA,yA,xB,yB,xC,yC,xD,yD]T=

[286.93,182.33,370.59,184.52,368.22,

284.01,285.18,275.86]T

(24)

期望圖像特征點像素位置為

[453.82,78.35,445.59,423.12,148.17,

420.77,156.06,70.69]T

(25)

機械臂的初始角度為

(26)

4.2 實驗結(jié)果分析

在機器人仿真實驗中，設(shè)λ=0.5，并在實驗過程中按照經(jīng)驗加入均值為0、方差為0.2的隨機擾動噪聲。同時為了進一步說明提出算法的優(yōu)越性，與現(xiàn)已取得較好性能的KFRBF算法及經(jīng)典KF算法做了對比實驗。本文采用收斂速度(圖像特征誤差達到收斂閾值時的迭代次數(shù))和圖像特征累積誤差2個性能指標來評價機械手視覺伺服控制模型的性能。

(27)

(28)

式中：dc為圖像特征誤差達到最小值時的迭代次數(shù)；d為迭代次數(shù)，1≤d≤dc；e(d)為第d次迭代的圖像特征誤差；eLAE為圖像特征累積誤差。

4.2.1 與其他方法的對比實驗

(a)圖像特征軌跡

(a)圖像特征軌跡

(a)圖像特征軌跡

4.2.2 魯棒性實驗

為了驗證本文算法對不同噪聲干擾的魯棒性，又進行了第2組實驗，在實驗中，加入方差分別為0.3和0.4的隨機擾動，3種算法的實驗仿真結(jié)果如圖7—圖12所示, 3種算法的收斂速度和累積誤差仿真結(jié)果見表1。

將2種方差下的結(jié)果與第1組實驗一起對比可以得出IBVS在同一算法、不同擾動誤差下的性能。從圖4(a)—圖12(a)可看出，當噪聲方差分別為0.2，0.3，0.4時，3種算法圖像特征軌跡都無明顯變化，圖像空間特征全都穩(wěn)定在相機視場范圍內(nèi)。從圖4(b)、圖7(b)、圖10(b)可以看出，當系統(tǒng)擾動誤差由0.2增加到0.3，又由0.3增加到0.4時，末端執(zhí)行器位姿軌跡誤差無明顯變化。從圖5(b)、圖8(b)、圖11(b)以及圖6(b)、圖9(b)、圖12(b)可看出，基于KFRBF算法和基于KF算法的IBVS控制模型在擾動誤差發(fā)生改變時，其末端執(zhí)行器位姿軌跡誤差發(fā)生了較大變化，而基于KFLSTM算法的IBVS控制模型具有末端執(zhí)行器最平滑的運動軌跡，其運動最平穩(wěn)。另外，從表1可看出，當擾動誤差為0.2時KFLSTM、KFRBF、KF算法的迭代次數(shù)分別為100、148、202，相對于KF算法，KFLSTM算法收斂速度提高了102%，相較于KFRBF算法提高了48%。同理，擾動誤差為0.3時可計算出KFLSTM算法在收斂速度上相較于KF算法提高了122%，相較于KFRBF提高了66%。擾動誤差為0.4時，KFLSTM算法的收斂速度比KF算法、KFRBF算法分別提高了142%和75%。從定量的結(jié)果分析可以看出，同一擾動誤差下，3種對比算法中KFLSTM算法收斂速度最快，且累積誤差最小。另外，表1中的性能指標結(jié)果顯示，KFLSTM算法在不同噪聲方差下的迭代次數(shù)分別為100、98和102，累積誤差分別為8.53×103、8.53×103、8.54×103像素,無明顯的誤差變化，而另外2種算法只在累積誤差方面具有穩(wěn)定性，因此，與其他算法相比，基于KFLSTM算法的IBVS對噪聲具有更強的魯棒性。

表1 KFLSTM，KFRBF，KF算法仿真結(jié)果

(a)圖像特征軌跡

(a)圖像特征軌跡

(a)圖像特征軌跡

(a)圖像特征軌跡

(a)圖像特征軌跡

(a)圖像特征軌跡

實驗結(jié)果表明，基于KFLSTM算法的無標定視覺伺服控制模型中圖像誤差收斂均在0.5像素內(nèi)，說明模型定位精度較高，且對噪聲具有較強的魯棒性，在收斂速度、機械手末端執(zhí)行器的空間運動軌跡和特征誤差等方面均表現(xiàn)出了較好的性能。

5 結(jié)語

針對礦用智能巡檢機器人視覺伺服控制中采用傳統(tǒng)的KF算法圖像雅可比矩陣存在估計值不準確、魯棒性差的問題，提出了一種基于KFLSTM算法的無標定視覺伺服控制模型，利用LSTM對KF算法得到的次優(yōu)狀態(tài)進行誤差補償，進而獲得雅可比矩陣最優(yōu)估計值。KFLSTM算法通過提高雅可比矩陣估計值的準確性和穩(wěn)定性來改善視覺伺服控制的實時性和魯棒性。將KFLSTM算法應(yīng)用于六自由度機器人視覺伺服控制中，仿真結(jié)果表明，利用KFLSTM算法得到的圖像特征與傳統(tǒng)的KF算法以及KFRBF算法相比，誤差更小，誤差收斂速度相較于傳統(tǒng)KF算法提高了100%～142%，且機器人末端執(zhí)行器運動平穩(wěn)，具有較強的抗噪聲干擾能力。KFLSTM算法可有效提高巡檢機器人的作業(yè)精度與效率，并增強其工作的穩(wěn)定性與安全性。