基于前饋徑向基網(wǎng)絡(luò)的動態(tài)抓取參數(shù)估計方法

王建強，黃開啟，蘇建華

1）江西理工大學(xué)電氣工程與自動化學(xué)院，江西贛州 341000；2）中國科學(xué)院自動化研究所復(fù)雜系統(tǒng)管理與控制國家重點實驗室，北京 100190

抓取運動的目標(biāo)物體是機器人領(lǐng)域中的難題之一，在航天應(yīng)用及工業(yè)生產(chǎn)等場合有廣泛的應(yīng)用需求.為保證抓取可靠性和穩(wěn)定性，機器人需通過視覺實時跟蹤運動目標(biāo)并估計出目標(biāo)的運動參數(shù).近年來，主流的目標(biāo)跟蹤方法包括增量視覺跟蹤（incremental visual tracking，IVT）［1］和循環(huán)結(jié)構(gòu)核（circulant structure kernel，CSK）［2］的生成式搜索算法、跟蹤學(xué)習(xí)檢測（tracking learning detection，TLD）算法［3］和核相關(guān)濾波（kernel correlation filter，KCF）［4］的判別式模型算法等.現(xiàn)有的運動參數(shù)估計方法有mean shift 算法［5］、Cam shift 算法［6］、粒子濾波［7］和卡爾曼濾波（Kalman filter，KF）等.Mean shift 算法和Com shift 算法需要較大的目標(biāo)與背景的對比度，通過圖中顏色直方圖分布確定目標(biāo)運動狀態(tài).KF是將理論預(yù)測與實際觀測融合得到更準(zhǔn)確的估計，但傳統(tǒng)KF 運動估計方法僅適于統(tǒng)計特性已知的白噪聲［8］，對統(tǒng)計特性未知的有色噪聲存在很大的估計誤差.JULIER 等［9］將 KF 推廣到非線性空間，將連續(xù)的非線性方程線性化和離散化，提出擴展卡爾曼濾波（extended Kalman filter，EKF），但存在如下缺點：①非線性模型的雅可比矩陣計算量大且難解出；②隨著時間延長，標(biāo)稱狀態(tài)會偏離系統(tǒng)實際狀態(tài)，造成估計精度降低；③若系統(tǒng)的誤差傳播函數(shù)不能用線性函數(shù)逼近，則會導(dǎo)致濾波器發(fā)散.JULIER等［10］將近似概率分布代替近似非線性函數(shù)提出了無跡卡爾曼濾波（unscented Kalman filter，UKF），采用unscented非線性變換進行模型的狀態(tài)與誤差協(xié)方差的遞推和更新，但是若UKF系統(tǒng)狀態(tài)的后驗概率密度是非高斯，則濾波效果很差［11］.CHOUKROUN 等［12］利用哈密頓算子將四元數(shù)狀態(tài)方程改寫成線性形式，提出四元數(shù)卡爾曼濾波（quaternion Kalman filter，QKF），但該方法需要頻繁計算四元數(shù)與誤差修正羅德里格斯參數(shù)之間的切換［13］，以保證四元數(shù)維度與濾波方差維度相同［14］.

在抓取運動目標(biāo)的研究中，安旭文［15］將機器人的控制過程分為跟蹤預(yù)測和捕捉兩個階段.在跟蹤預(yù)測階段，機器人處理圖像噪聲并用改進的最大類間方差法（Otsu's method）進行目標(biāo)檢測，而在捕捉階段則使用基于速度補償?shù)囊晥隹刂扑惴?但該方法缺少觀測量，且狀態(tài)量對模型的依賴較大；此外，該方法使用的粒子濾波對非線性非高斯的系統(tǒng)的濾波效果不好，無法滿足實時性要求.

針對機器人對二維平面中隨機運動物體的運動參數(shù)估計問題，通過前饋徑向基函數(shù)（radial basis function，RBF）［16］神經(jīng)網(wǎng)絡(luò)預(yù)測目標(biāo)運動采樣時間，迭代優(yōu)化求解最小損失函數(shù)來擬合非線性系統(tǒng)［17］.將神經(jīng)網(wǎng)絡(luò)的時間預(yù)測結(jié)果代入KF 的狀態(tài)方程，規(guī)劃機器人的最優(yōu)抓取路徑.相比傳統(tǒng)的機器人攔截式抓取方法［18-20］，本研究首次提出KCF與參數(shù)預(yù)估結(jié)合的機器人抓取方法，提高了抓取成功率.該方法繞開了非線性運動預(yù)測計算量大且不易求得奇異矩陣的難點，對目標(biāo)的突變有更好地適應(yīng)性，基于前饋RBF 的KF 運動預(yù)測抓取方法與EKF、QKF和UKF 方法相比，提高了抓取的成功率，與前饋感知機網(wǎng)絡(luò)的KF 方法相比，提高了目標(biāo)跟蹤的精度與抓取的效率.

1 系統(tǒng)概述

本研究提出的機器人抓取運動目標(biāo)的工作流程如圖1所示.通過KCF算法提取目標(biāo)特征，將相機提取到的信息分為平面信息與深度信息.平面信息抽取第1 幀制作目標(biāo)模板，第2 幀開始進行模板目標(biāo)匹配，從而得到目標(biāo)物體在平面上的實時位置，將其與深度信息結(jié)合，并通過機器人手-眼坐標(biāo)變換，可獲得目標(biāo)物體在機器人坐標(biāo)系下的三維坐標(biāo).然后，將此時末端手爪與目標(biāo)的運動速度，以及兩者之間的距離輸入到訓(xùn)練好的神經(jīng)網(wǎng)絡(luò)模型中，可以預(yù)測KF 狀態(tài)方程中的采樣時間.通過基于前饋神經(jīng)網(wǎng)絡(luò)的KF 預(yù)估下一采樣目標(biāo)的位置，可以實時跟蹤并抓取目標(biāo).

1.1 KCF跟蹤算法

KCF算法的思想是將所有輸入圖像特征的循環(huán)矩陣回歸到傅里葉空間并對其進行對角化運算，此過程不需要目標(biāo)外觀的硬閾值樣本，因此具有計算量小、檢測準(zhǔn)確性高的特點，滿足對實時性要求較高的目標(biāo)跟蹤需求.

KCF 算法的目標(biāo)跟蹤過程如圖2所示，具體檢測過程為：①在第1 幀給定的目標(biāo)位置提取圖像塊，訓(xùn)練后得到相關(guān)濾波器.在后續(xù)的每一幀中，根據(jù)前一幀的位置提取新的圖像塊，用于目標(biāo)檢測.②提取圖像塊的方向梯度直方圖（histogram of oriented gradient，HOG）特征，并用余弦窗口平滑邊緣.③采用離散傅里葉變換（discrete Fourier transform，DFT）執(zhí)行相關(guān)濾波操作.④采用離散傅里葉逆變換（inverse DFT，IDFT）得到置信圖，置信圖最大值所對應(yīng)的坐標(biāo)位置即為目標(biāo)的新位置，并由此估計位置訓(xùn)練和更新相關(guān)濾波器.其中，⊙為卷積操作符.

2.2 卡爾曼運動估計算法

本研究主要針對的是待抓取的物體為非勻速、任意方向運動的目標(biāo).首先，判斷機器人末端手爪與目標(biāo)物體兩者中心點的距離，如圖3紅色虛線所示.然后，根據(jù)兩幀之間的時間和紅點的位移算出目標(biāo)瞬時速度與加速度來判斷物體的運動方向.

圖3 機器人末端運動與目標(biāo)物體運動關(guān)系圖Fig.3 Relationship between manipulator end motion and target object motion

預(yù)判抓取時間有利于讓機器人在設(shè)定的邊界條件下到達抓取點.若物體速度超過機器人抓取允許的最大值，則需延長估計KF狀態(tài)方程的采樣時間.為使物體運動軌跡與手爪軌跡交匯于某點，需根據(jù)物體當(dāng)前的速度和加速度計算機器人末端的速度和加速度.由于物體的速度與方向是隨機的，為避免因目標(biāo)運動突變而導(dǎo)致的預(yù)測失效，需以適當(dāng)?shù)念l率在機器人抓取路徑上均勻插值.

假設(shè)物體以恒定加速度直線運動，若物體到達抓取點xf所需的時間（亦稱抓取時間）為tf，則

其中，xm、vm和am分別是當(dāng)前物體的位置、速度和加速度.

假設(shè)物體在x軸上的捕捉距離D=xf-xm，若vm＞ 0且則最小捕捉時間為

若am很小，則tf可通過假設(shè)恒定速度軌跡來計算，由式（1）可得xf=xm+vmtf，由xf=xm+vmtf和D=xf-xm可得通過規(guī)劃機器人的運動軌跡，可使手爪經(jīng)過時間tf時準(zhǔn)確到達抓取點.

在數(shù)字圖像采集中，由于相鄰兩幀的采集時間間隔非常短，且物體的運動距離也非常短，因此在理想狀態(tài)下，可假定目標(biāo)在短時間內(nèi)做勻速運動，故用位置與速度可表達運動狀態(tài)向量.用一個4維向量 (sx，k，sy，k，vx，k，vy，k)表示目標(biāo)在x和y方向的位移和速度在k時刻的狀態(tài)，則運動方程為

其中，sx，k和sy，k分別為k時刻目標(biāo)中心在x方向和y方向的位移；vx，k和vy，k分別為k時刻目標(biāo)在x方向和y方向的速度；Δt=tk-tk-1.

本研究建立的KF狀態(tài)方程為

其中，ω為4 × 1維的系統(tǒng)噪聲向量；Δt為KF方程的采樣時間.

KCF 在采樣周期內(nèi)返回k時刻圖像目標(biāo)中心點位置 (sx，k，sy，k)，故觀測方程的前半部分是一個 2 × 2維的對角矩陣，即

其中，v為 2 × 1 維的觀測噪聲向量；分別為k時刻目標(biāo)中心在x方向和y方向的位移觀測值.

通過KCF 算法檢測出目標(biāo)中心的位置(sx，k-1，sy，k-1），用倒數(shù)第 1、14 和 27 幀中目標(biāo)的位移差除以兩幀之間的時間差，計算出速度vx，k和vy，k，并組成系統(tǒng)初始狀態(tài)以初始化KF方程.將觀測向量帶入KF狀態(tài)方程并進行更新狀態(tài)，得到不同時刻運動目標(biāo)的位置和速度的估計值.利用改進的卡爾曼濾波器預(yù)測目標(biāo)運動的位置和速度.將式（5）的觀測向量代入狀態(tài)方程，實現(xiàn)濾波器狀態(tài)更新.經(jīng)過這樣的預(yù)估-校正過程，用卡爾曼濾波器估計出目標(biāo)在未來某一時刻的位置，根據(jù)該位置規(guī)劃機器人的運動軌跡和速度，并通過夾爪的開合判斷是否抓取成功.

2 基于前饋徑向基網(wǎng)絡(luò)的運動參數(shù)預(yù)測與抓取

在目標(biāo)速度隨機變化情況下，為實現(xiàn)機器人穩(wěn)定抓取，本研究提出一種基于前饋RBF神經(jīng)網(wǎng)絡(luò)動態(tài)預(yù)測采樣時間Δt的卡爾曼濾波算法.圖4給出機器人運動與相機采樣周期對應(yīng)關(guān)系的示例.其中，黑色實心點是機器人每一輪運動和相機采樣的起始位置；綠色空心點是需要預(yù)測的目標(biāo)位置；相機采樣周期是30 ms，機器人每次運動的時間與它運動的距離和速度有關(guān).機器人在其一個運動周期內(nèi)，將不接受上位機的指令，需等到下一個運動周期的到來才能接受指令，故相機采樣周期與機器人運動周期的差值（即KF的采樣時間）是動態(tài)變化的.

圖4 機器人運動周期與相機的采樣周期（單位：ms）Fig.4 Robot arm movement and camera sampling period(unit:ms)

常用的感知機神經(jīng)網(wǎng)絡(luò)通過權(quán)連接輸入層與隱藏層，RBF神經(jīng)網(wǎng)絡(luò)將輸入矢量直接映射到以RBF為隱單元的“基”所構(gòu)成的隱空間上.輸入到輸出的映射是非線性的，隱含層到輸出的映射是線性的，在隱含層，把低維度線性不可分向量映射到高維度成線性可分向量.通過在高緯度可調(diào)參數(shù)（權(quán)值和閾值）與輸出之間關(guān)系得到線性方程組可求得網(wǎng)絡(luò)的權(quán)值.RBF 神經(jīng)網(wǎng)絡(luò)的優(yōu)點有：①提高訓(xùn)練速度與學(xué)習(xí)效率；②克服了在低維度陷入局部極小值的問題；③收斂速度比前一個網(wǎng)絡(luò)快，輸入數(shù)據(jù)與所選中心點中起主要作用的是與輸入數(shù)據(jù)很近的點，且局部逼近簡化計算量，局部逼近比全局逼近學(xué)習(xí)速度更快.圖5是神經(jīng)網(wǎng)絡(luò)網(wǎng)絡(luò)在系統(tǒng)中對輸入聚類和訓(xùn)練輸出的信息流程.

圖5 系統(tǒng)框架及其RBF網(wǎng)絡(luò)信息流圖Fig.5 RBF network information flow diagram and its system framework

RBF神經(jīng)網(wǎng)絡(luò)的激活函數(shù)為

其中，xp是第p個輸入樣本的坐標(biāo)；ci是第i個中心點的坐標(biāo)；σ是函數(shù)的寬度參數(shù)，用于控制函數(shù)的徑向作用范圍.

RBF神經(jīng)網(wǎng)絡(luò)的輸出為

其中，n為輸出的樣本數(shù)；h為隱含層的結(jié)點數(shù)；wi為第i個輸出神經(jīng)元的權(quán)重.在本研究中，y即采樣時間Δt，代入KF的狀態(tài)方程得

利用最小二乘法計算隱藏層神經(jīng)元與輸出層之間的連接權(quán)值wi，并設(shè)cmax為所選取中心點之間的最大距離，則

其中，P為聚類中心的個數(shù).

深度神經(jīng)網(wǎng)絡(luò)雖然適合用來進行時序建模，但其體系結(jié)構(gòu)缺乏直觀的高階時空架構(gòu).繁雜的訓(xùn)練過程和中央處理器（central processing unit，CPU）資源的大量占用導(dǎo)致機器人抓取過程中的實時性降低.

3 實驗與分析

為驗證本研究方法的優(yōu)越性，選取兩種神經(jīng)網(wǎng)絡(luò)預(yù)測KF 的采樣時間，并在自建的數(shù)據(jù)集上用相同數(shù)據(jù)集進行訓(xùn)練，對比分析兩者的抓取效果.實驗的硬件配置為Intel Core i7-10875 2.3 GHz CPU，16 Gbytes 內(nèi) 存 ， GDDR6 顯 卡 ， 操 作 系 統(tǒng) 為Ubuntu16.04，輸入圖像分辨率為1 280 × 960像素，使用Python 語言在pycharm 中搭建手眼機器人的操作系統(tǒng).

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

基于本研究的抓取穩(wěn)定與高效性的評估方法，分別比較感知機與RBF神經(jīng)網(wǎng)絡(luò)預(yù)測時間參數(shù)，結(jié)合KF 估計運動參數(shù)，在UR5 機器人進行追蹤并抓取機動目標(biāo)實驗，分析不同算法的性能.實驗所用UR5 機器人實物如圖6（a）.機器人首先對在單方向、非勻速的狀態(tài)下的目標(biāo)物體進行抓取，即由操作者隨機拉著目標(biāo)做變速直線運動（圖6（b）），然后對如圖6（c）中任意方向下的目標(biāo)物體進行抓取.

圖6 UR5機器人實物圖與試驗機器人單方向非勻速運動模式Fig. 6 The physical map of the UR5 manipulator and the one-direction non-uniform motion mode of the test robot

圖7為采用EKF、UKF和QKF算法抓取非勻速且隨機運動的物體時的過程曲線.由圖7可見，采用EKF、UKF和QKF算法進行參數(shù)預(yù)估的方法，目標(biāo)抓取成功率很低，大多數(shù)情況下是失敗的.

圖7 采用（a）EKF、（b）UKF和（c）QKF預(yù)測算法對非勻速隨機運動物體的抓取過程Fig.7 The grasping process of non-uniform random moving objects predicted by(a)EKF,(b)UKF,and(c)QKF

將平面抓取運動分解到x軸與y軸.圖8（a）中，采用感知機神經(jīng)網(wǎng)絡(luò)，手爪與目標(biāo)在x軸方向自開始抓取點起，后面的最大距離不超過10 cm，這是由于當(dāng)機器人結(jié)束一輪運動周期時，物體突然加速，UR5機器人的最大加速度設(shè)為0.35 m/s2.如果突然施加的瞬時加速度超過當(dāng)前運動速度，機器人會在下一輪運動周期由兩者位置、速度與加速度的差值，通過神經(jīng)網(wǎng)絡(luò)前饋時間預(yù)測的KF 加大預(yù)測量再次進行抓取.在后面的跟蹤抓取中，機器人依據(jù)物體移動的軌跡實時規(guī)劃自己的路徑，直至抓取成功.圖8（b）是在RBF 神經(jīng)網(wǎng)絡(luò)預(yù)測采樣時間參數(shù)下的情況，機器人手爪和目標(biāo)在x軸上的坐標(biāo)變化情況.由圖可見，前饋RBF 神經(jīng)網(wǎng)絡(luò)的KF 預(yù)測使UR5在開始抓取點后追蹤過程中與目標(biāo)距離最大不超過5 cm.可見，感知機經(jīng)網(wǎng)絡(luò)預(yù)測跟蹤算法雖然在時間預(yù)測上對于非線性的運動有很好的適應(yīng)性，但是在某些點上還是會有較大落后.相比之下，采用RBF神經(jīng)網(wǎng)絡(luò)的路徑規(guī)劃，在跟蹤過程會更平滑，機器人運動路徑規(guī)劃中的插值也更均勻，此過程對于目標(biāo)下一步運動狀態(tài)發(fā)生突變而能及時做出調(diào)整.圖8（c）和（d）顯示了抓取過程中y軸上的跟蹤誤差.由圖8（c）和（d）可見，在時長4 s的抓取過程中，RBF神經(jīng)網(wǎng)絡(luò)預(yù)測方法抓取到目標(biāo)所用的時間比感知機方法少0.8 s，抓取效率提高了20%.對于目標(biāo)物體在加速或減速（曲線的轉(zhuǎn)折處代表速率發(fā)生了大突變）情況下的目標(biāo)跟蹤，兩種神經(jīng)網(wǎng)絡(luò)比傳統(tǒng)的位置伺服方法都更具靈活性，這表明神經(jīng)網(wǎng)絡(luò)對于KF 狀態(tài)方程的時間預(yù)測更具優(yōu)越性.RBF神經(jīng)網(wǎng)絡(luò)預(yù)測方法對于單方向上變速運動的靈敏性更好，能在采樣周期內(nèi)規(guī)劃好跟蹤的最佳路徑，達到更穩(wěn)定跟蹤和更高效抓取的效果.

圖8 感知機神經(jīng)網(wǎng)絡(luò)和RBF神經(jīng)網(wǎng)絡(luò)在x軸和y軸抓取變速直線運動物體的過程Fig.8 The x axisand y axis grabbing process of change speed straightline motion objectby perceptronandRBF neuralnetworks

圖9為分別采用前饋感知機神經(jīng)網(wǎng)絡(luò)和RBF神經(jīng)網(wǎng)絡(luò)的KF 參數(shù)預(yù)估算法抓取非勻速任意方向運動的物體（圖7（c））時的過程曲線.由圖9可見，采用感知機神經(jīng)網(wǎng)絡(luò)方法預(yù)測出現(xiàn)了跟蹤不良的情況，在急加速或急轉(zhuǎn)彎的情況下，跟蹤甚至?xí)a(chǎn)生較大的誤差，導(dǎo)致誤抓取從而降低了抓取的效率.

圖9 感知機神經(jīng)網(wǎng)絡(luò)和RBF神經(jīng)網(wǎng)絡(luò)在x軸和y軸抓取非勻速運動物體的過程Fig.9 The x axis and y axis grabbing process of non-uniform movement motion object by perceptron and RBF neural networks

圖10為前饋神經(jīng)網(wǎng)絡(luò)KF對任意方向做非勻速運動的目標(biāo)進行抓取的3 維仿真結(jié)果.由圖10 可見，前饋RBF 的KF 預(yù)測對目標(biāo)的預(yù)測在運動速度突變或方向突變的情況下也能及時做出調(diào)整并穩(wěn)定跟蹤，其對機動目標(biāo)運動的預(yù)判性，及對機器人跟蹤路徑上預(yù)測點插值的均勻性，對一些急速變化的目標(biāo)也能緊緊跟隨，精準(zhǔn)抓取.在時間預(yù)測與路徑規(guī)劃上，基于RBF神經(jīng)網(wǎng)絡(luò)改進的卡爾曼濾波規(guī)劃的跟蹤路徑及抓取都優(yōu)于感知機神經(jīng)網(wǎng)絡(luò)時間預(yù)測的卡爾曼濾波.

圖10 采用感知機神經(jīng)網(wǎng)絡(luò)和徑向基神經(jīng)網(wǎng)絡(luò)進行抓取的3維仿真結(jié)果Fig.10 Three-dimensional grasping process by using perceptron and RBF network networks

結(jié) 語

本研究提出一種新的運動參數(shù)估計方法，設(shè)計了基于徑向基函數(shù)神經(jīng)網(wǎng)絡(luò)動態(tài)規(guī)劃卡爾曼濾波采樣時間的方案，并結(jié)合視覺伺服控制器規(guī)劃UR5機器人運動路徑、速度與加速度，在保證抓取效率的同時，大幅提高了機器人抓取的精度.通過自建數(shù)據(jù)庫訓(xùn)練RBF 網(wǎng)絡(luò)模型，并提升了模型的泛化能力.機器人抓取動態(tài)目標(biāo)的實驗結(jié)果，驗證了本研究方法比起傳統(tǒng)卡爾曼濾波算法能更有效地提高跟蹤的穩(wěn)定性、抓取成功率和抓取效率.

未來計劃采用上述多種典型方法進行多種組合方式進行方案的測試，并結(jié)合3維空間跟蹤算法與機器人的視覺伺服控制器，實現(xiàn)機器人在3維空間的動態(tài)抓取.