礦用智能巡檢機器人無標定視覺伺服控制研究

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

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

0 引言

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

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

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

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

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

1.1 圖像雅可比矩陣模型

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

定義圖像特征誤差為

e=S-S*

(1)

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

圖像特征在IBVS系統中的變化率與關節角的變化率之間的映射關系為

(2)

機械臂關節角變化率與圖像雅可比矩陣之間的關系為

(3)

(4)

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

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

視覺空間-運動空間滿足非線性映射關系：

(5)

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

1.2 KF算法分析

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

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

(6)

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

(7)

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

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

(8)

(9)

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

預測過程公式為

(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是一種對高斯白噪聲下的線性系統的狀態變量進行最優估計的有效方法。

2 改進KF算法

KF是在環境噪聲為高斯白噪聲下的最優線性狀態估計，但是在機器人運動的真實環境中由機器人末端執行器引入的噪聲并不是簡單的高斯白噪聲，這就導致了KF的估計變成了次優估計。為了得到最優估計，本文提出了 LSTM網絡誤差補償模型，實現了非線性濾波動態補償。

(15)

2.1 LSTM構建

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

圖1 LSTM結構

LSTM在實現信息傳遞的過程中首先是將當前單元的狀態輸入和前一個單元的隱藏層向量經過遺忘門輸出一個遺忘向量，遺忘門將刪除不相關的信息。該過程的實現方程為

(16)

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

(17)

(18)

(19)

當前單元細胞的長期記憶會通過一個輸出門結構得到當前輸出信息和當前隱藏層信息。該過程的實現方程為

(20)

(21)

2.2 神經網絡誤差補償模型

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

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

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

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

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

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

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

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

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

(22)

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

4 實驗結果與分析

4.1 實驗條件與參數設置

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

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

(23)

設置初始圖像特征點像素位置為

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 實驗結果分析

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

(27)

(28)

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

4.2.1 與其他方法的對比實驗

(a)圖像特征軌跡

(a)圖像特征軌跡

(a)圖像特征軌跡

4.2.2 魯棒性實驗

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

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

表1 KFLSTM，KFRBF，KF算法仿真結果

(a)圖像特征軌跡

(a)圖像特征軌跡

(a)圖像特征軌跡

(a)圖像特征軌跡

(a)圖像特征軌跡

(a)圖像特征軌跡

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

5 結語

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