礦用智能巡檢機(jī)器人無標(biāo)定視覺伺服控制研究

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

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

0 引言

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

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

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

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

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

1.1 圖像雅可比矩陣模型

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

定義圖像特征誤差為

e=S-S*

(1)

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

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

(2)

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

(3)

(4)

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

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

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

(5)

式中ve(t)為t時刻機(jī)器人末端執(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)的白噪聲矩陣。

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

(8)

(9)

基于KF的狀態(tài)估計可分為2步進(jìn)行：第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)變量進(jìn)行最優(yōu)估計的有效方法。

2 改進(jìn)KF算法

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

(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個門單元，它們分別是輸入門、遺忘門和輸出門。輸入門表示要保存的信息或者待更新的信息，遺忘門決定丟棄哪些信息，輸出門決定當(dāng)前神經(jīng)元細(xì)胞輸出的隱向量。在本文中，LSTM起到了精確的誤差估計器的作用，其任務(wù)是將期望狀態(tài)與估計狀態(tài)之間的誤差最小化。LSTM結(jié)構(gòu)如圖1所示。

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

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

(16)

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

(17)

(18)

(19)

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

(20)

(21)

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

圖2 基于LSTM的誤差補(bǔ)償模型

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

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

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

3 基于改進(jìn)KF算法的無標(biāo)定視覺伺服模型

為了在無標(biāo)定視覺伺服控制中獲得更準(zhǔn)確的雅可比矩陣估計值，采用了基于LSTM補(bǔ)償?shù)腒F算法(KFLSTM算法)，通過該算法獲得雅可比矩陣最優(yōu)值，進(jìn)而得到雅可比矩陣廣義逆與特征誤差矩陣的乘積，使控制器輸出準(zhǔn)確的關(guān)節(jié)角速度，從而控制機(jī)械臂的實時運行，提高視覺伺服控制的精度和收斂速度?；贙FLSTM算法的無標(biāo)定視覺伺服控制模型框架如圖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算法的無標(biāo)定視覺伺服控制模型框架

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

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

(22)

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

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

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

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

在仿真實驗中，選取某一物體上的4個點即A，B，C，D的空間坐標(biāo)作為無標(biāo)定視覺伺服控制模型的目標(biāo)特征點，且特征點在笛卡爾坐標(biāo)系中的坐標(biāo)為P，初始圖像特征為S0，期望圖像特征為S*，機(jī)械臂的初始角度為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)

機(jī)械臂的初始角度為

(26)

4.2 實驗結(jié)果分析

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

(27)

(28)

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

4.2.1 與其他方法的對比實驗

(a)圖像特征軌跡

(a)圖像特征軌跡

(a)圖像特征軌跡

4.2.2 魯棒性實驗

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

將2種方差下的結(jié)果與第1組實驗一起對比可以得出IBVS在同一算法、不同擾動誤差下的性能。從圖4(a)—圖12(a)可看出，當(dāng)噪聲方差分別為0.2，0.3，0.4時，3種算法圖像特征軌跡都無明顯變化，圖像空間特征全都穩(wěn)定在相機(jī)視場范圍內(nèi)。從圖4(b)、圖7(b)、圖10(b)可以看出，當(dāng)系統(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可看出，當(dāng)擾動誤差為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中的性能指標(biāo)結(jié)果顯示，KFLSTM算法在不同噪聲方差下的迭代次數(shù)分別為100、98和102，累積誤差分別為8.53×103、8.53×103、8.54×103像素,無明顯的誤差變化，而另外2種算法只在累積誤差方面具有穩(wěn)定性，因此，與其他算法相比，基于KFLSTM算法的IBVS對噪聲具有更強(qiáng)的魯棒性。

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

(a)圖像特征軌跡

(a)圖像特征軌跡

(a)圖像特征軌跡

(a)圖像特征軌跡

(a)圖像特征軌跡

(a)圖像特征軌跡

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

5 結(jié)語

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