基于KALMAN濾波和深度學習的機器人飛拍方法研究*

田志彬 謝小輝

(蘇州大學機電工程學院， 江蘇 蘇州 215100)

機器視覺取代人眼對環境進行感知，通過對目標工件形狀、姿態及位置識別與定位目標，使工業生產線更加靈活、智能，解決傳統工業離不開人工檢測的問題[1]。在工業生產中，效率是一個重要的評價指標，飛拍定位是在動態過程中解決糾偏問題的方法，可以顯著提高效率，所以飛拍在工業上的應用具有良好的發展前景。咸浩等人[2]基于機器視覺設計圓角袋單切系統，采用飛拍模式的視覺系統圖像處理速度快，有效解決傳統生產圓角袋時出現的效率低、廢料多、機械磨損快等問題。黎揚進等人[3]提出一種卡爾曼—粒子濾波算法，有效提高了焊縫的跟蹤精度。在移動對象軌跡預測高精度要求的應用領域中，基于軌跡擬合的傳統預測算法已無法精準地預測出運動行為動態變化的移動對象的軌跡位置[4]。

本文擬針對對動態抓取的工件進行飛拍定位糾偏的場景，設計了視控一體化系統，改進卡爾曼濾波精準預測相機觸發時機器人位置，設計ResNet50為基本框架的神經網絡實現快速的圖像處理，最終實現高精度、高效率的飛拍定位。

1 視覺-控制一體化實時系統搭建

傳統的基于視覺的機器人控制系統，視覺與控制模塊分離大都采用分離方案，相互之間通過TCP/IP協議實現數據通訊，實時性差；并且控制柜占用較大空間，視覺系統不輕便。結合現有機器人基于視覺的控制系統的不足，設計一種實時性、硬件系統簡捷的視覺控制一體化系統，系統組成如圖1。

在嵌入式工控機Windows系統上植入Codesys RTE，使工控機變為實時的控制系統，控制機器人的運動。同時在工控機上設計視覺系統，通過共享內存的快速交互方式獲取機器人實時信息。通過Ethercat的通訊方式實現對機器人的運動控制，在一個通訊周期過程中，從站對數據幀的處理完全由硬件實現，所以延時很短，約為100～500 ns，這極大程度上保證了實時性。系統可以實現對物件的動態抓取以及飛拍定位。

2 算法的設計與分析

2.1 改進卡爾曼算法原理

植入Codesys RTE的實時系統實現機器人的運動控制，每隔Tms讀取機器人的位置，并將位置信息通過共享內存的通訊方式傳送到視覺控制端。由于時間間隔滿足不了位置觸發精度，所以通過改進卡爾曼預測算法精準預測下一個Tms時間點的位置，提前鎖定觸發位置所在的時間段，通過實時更新的運動方程對時間段進行分割，得到觸發位置的時間點。通過C++庫chrono實現對程序段的微秒級高精度計時和高精度延時，從而實現精準觸發。卡爾曼算法示意圖如圖2。

卡爾曼濾波[5]是一種利用線性系統狀態方程，通過系統的輸入輸出觀測數據，對系統狀態進行最優估計的算法。將卡爾曼濾波算法應用于移動對象動態軌跡預測具有實時性高的優勢，對頻繁變換運動狀態的移動對象具有較高的自適應性，是一種普適的機器學習方法[6]。

卡爾曼濾波動態軌跡預測系統的狀態方程和觀測方程，如下所示:

系統的狀態方程(預測方程)：

xk=Ak×xk-1+Bk×uk+wk

(1)

觀測方程：

zk=Hk×xk+vk

(2)

式中：xk是狀態向量；Ak是狀態轉移矩陣；Bk是控制變量的矩陣；uk是狀態控制向量。

由于傳統的卡爾曼預測應用此場景時有較大位置預測誤差，所以對傳統的卡爾曼濾波進行改進。前幾次迭代中還是應用傳統的時間更新公式：

(3)

(4)

誤差穩定之后，引入反饋向量sss=[ss0]，ss為前一時刻先驗估計位置預測誤差。

(5)

(6)

對于變加速運動，在測量更新中實時更新每段間隔的加速度，利用上一時間段的加速度，更新下一時間段的位移和速度。觀測更新方程如下：

(7)

(8)

(9)

前幾次迭代中，引入較小的加速度，穩定位置和速度：

(10)

誤差穩定并且引入反饋向量sss之后，更換加速度更新方程：

(11)

2.2 基于ResNet50網絡的深度學習網絡原理

深度學習目標檢測算法可以從功能上分為兩部分，其一是提取語義信息的骨架特征提取網絡，其二是對提取到的語義信息進行運算、得出最終結果的結果預測網絡。為了提高模型目標檢測的準確性，考慮選擇合適的骨架網絡和預測網絡進行合理組合與優化，并且建立合適的損失函數，結合動量法優化網絡。圖3為改進的網絡結構模型。

(1)部分是ResNet50骨架網絡，ResNet50是具有殘差結構的神經網絡。在解決梯度的過程中往往伴隨深度加深[7]，該網絡能有效防止在反向傳播過程中梯度彌散問題[8]，進而解決梯度變化時深度加深問題[9]。(2)部分是特征金字塔網絡，它是一種多尺度特征融合網絡，通過特征金字塔網絡融合高層特征和底層特征的優點，進行更準確的檢測。(3)部分對特征圖進行學習，通過優化函數調整過程參數較小損失函數值，對潛在目標進行定位和分類，通過置信度分析減少重疊的預測邊界框。

為了解決Hessian矩陣病態條件問題，引入動量優化算法，它能夠引導迭代方向朝著正確的方向進行。如果前一刻的梯度與當前的梯度方向差別很大，不加處理的話該時刻會有大幅徘徊的梯度。由于動量積攢了歷史的梯度，把歷史梯度疊加到當前時刻梯度上，從而使得當前時刻的梯度幅度減小。

更新公式：

vt=γ×vt-1+α?

(12)

θnew=θ-vt

(13)

公式(12)為動量更新公式,vt-1為之前所有步驟所累積的動量和，γ為動量參數，α為學習率，?為梯度，θ為待更新參數。

2.3 算法應用于飛拍的理論分析

脫離傳感器觸發的飛拍糾偏定位主要包括以下兩個過程：

(1)示教：設定一個相機觸發位置，通過改進的卡爾曼算法精準觸發相機拍照，通過深度學習識別圖像的ROI區域，對ROI區域圖像進行邊緣檢測及相關處理，得到精確的圖像位姿信息。最后示教機器人精確裝配工件，記錄機器人在裝配的位姿信息。得到的圖像和機器人位姿作為飛拍糾偏的模板信息。

(2)飛拍糾偏：確保在同一位置得到工件的圖像坐標，通過深度學習快速識別圖像的位姿信息，與模板信息進行偏差計算，得到的偏差就是工件的偏移信息。通過手眼標定，將工件的偏差轉換到機器人坐標下。在機器人下一節拍前，將偏差疊加到模板信息中生成新的機器人目標位置，實現對工件的高精度裝配。

3 實驗分析

3.1 改進卡爾曼算法實驗分析

卡爾曼濾波器的觀測量可以少于動態系統中狀態向量所包含的元素個數。所以測量向量只有一個位移變量，為一維向量。采樣時間為4 ms,把每隔4 ms實時采集的機器人Y軸位置作為測量向量zk的主要部分，創建狀態向量x=[sv]，Ak=[1,ΔT;0,1]，Bk=[ΔT2/2;ΔT]，Hk=[1,0],測量噪聲為±0.000 5，測量噪聲協方差R為0.000 5，過程噪聲協方差Q=[0,0;0,1]。取第一個數據點為0時刻，從第9個點開始迭代，預測結果圖見圖4。

由圖5可以看出，傳統的卡爾曼預測誤差達到穩定后波動較大，1.2 s后，最大誤差值達到0.059 7 mm,標準差為0.024 5 mm。如圖6，1.2 s以后，用上一時刻先驗估計誤差進行反饋，位置預測誤差明顯減小，誤差基本位于±0.02 mm內,并且最大誤差降低到0.037 6 mm,標準差為0.013 4 mm。與傳統方法相比，預測精度得到了明顯提高。

速度變化比較平穩，加速度雖然波動較大，但是加速度疊加在ΔT2=0.000 016上面，對于預測精度影響不大。因此前面可以利用較小的加速度穩定卡爾曼濾波的過程參數，后面更正加速度表達式，進行精準預測。

當改進的卡爾曼濾波算法預測到下一時間點的位置大于觸發的位置時，算法停止迭代。利用上一時間點的速度和預測速度，對上一時間點和預測時間點的這一時間段進行分割，找到觸發位置的時間點。由于時間段足夠短，加速度對位移基本沒有影響，分割過程可以按照機器人勻速運動處理。

時間段的分割公式：

(14)

利用chrono庫中system_clock::now()函數，設計微秒級計時和延時程序，用于對預測算法進行耗時計算和延時觸發。通過測試改進的卡爾曼濾波迭代一次的時間Tl不超過8 μs。所以精準延時時間為Te=T-Tl。

SCARA機器人空載，機器人運行最大速度設為150 mm/s,對改進的卡爾曼濾波進行精度驗證。由于機器人末端固定，并且采用左手系運動，其末端的圓心位置與機器人空間位置一一對應，即驗證每一幀圖像的中心位置坐標差值即可。

通過改進的卡爾曼濾波觸發相機采集圖像，進行200次實驗，機器人末端中心的Y軸圖像坐標如圖9所示。

由上述實驗數據得出，均方差為0.827 px,最大偏差為2.781 px。即均方差為0.017 378 mm,最大偏差為0.058 4 mm，觸發重復度滿足后續飛拍實驗的要求。

3.2 深度學習網絡訓練實驗分析

搭建網絡后對1 500張工件進行訓練，將70%作為訓練集，15%作為驗證集，15%作為測試集。最大訓練步數設置為200，學習率參數為0.000 5，動量參數為0.9。準確率最終穩定在95%左右，損失函數小于0.05，生成best.hdl文件，后面調用訓練好的網絡文件即可獲取訓練框的大小和方向。基于GPU型號為GeForece GTX1050ti的應用識別時間在50 ms以內，基于CPU應用時間在90 ms以內。由于在線實驗時工控機型號的限制，采用基于CPU的深度學習應用。圖10為訓練過程可視化圖。

圖10中，最上層為輸入圖像，中間層為標注圖像，最下層為訓練結果。

3.3 飛拍實驗分析

實驗采用海康工控機MV-VB2210-120G,驅動器為STEP四軸驅動器，相機分辨率為3 072 × 2 048，光源采用環形光源,SACRA大臂小臂長為200 mm。機器人位置(200,5.6,60)設為相機觸發位置，實時獲取機器人位置的周期設為4 ms。

按照前面所述的飛拍過程進行飛拍實驗的結果：

(1)示教：在機器人位置(200,5.6,60)處觸發的圖像像素坐標為(1 702.38,1 351.79)，姿態為12.32°；示教得到的機器人裝配時坐標為(200.52,202.73)，姿態為11.63°。

(2)飛拍糾偏：SCARA吸取芯片后，經過預測算法觸發相機拍照，然后利用神經網絡快速識別芯片的位姿，與步驟1得到的模板像素坐標進行偏差計算，然后將偏差值轉換到機器人坐標下，進而疊加到步驟1中的機器人示教坐標上得到精準的目標裝配位置。通過固定相機對放置結果進行誤差測量，把x方向和y方向誤差的平均值視為定位誤差。

實驗測試了機器人在不同速度下應用視控一體方法進行飛拍糾偏定位，每個速度下進行200次測試，實驗結果及實驗場景圖如圖11所示。

對圖11～圖14結果進行統計計算，計算結果如下表:

表1 實驗結果

機器人最大速度為70 mm/s時，平均定位誤差在±0.06 mm以內；最大速度為100 mm/s時，平均定位誤差在±0.065 mm以內；最大速度為170 mm/s時，平均定位誤差在±0.08 mm以內。并且最大定位誤差均不超過±0.11 mm。

4 結語

通過本文的研究，采用視控一體化的機器人飛拍系統較之傳統的視控分離方案，系統簡捷易用，具有實時性高的特點。提出一種無傳感器的精準時間觸發算法，結合深度學習實現精準的飛拍定位。經過1 000次的飛拍實驗測試及其數據，表明：通過采用深度學習算法，針對貼裝芯片工件的飛拍實驗，最快算法時間在50 ms以內，傳統的模板檢測時間為150 ms；通過采用改進型KALMAN濾波算法，較之傳統的卡爾曼濾波算法，在應用此場景時，預測誤差的標準差從0.024 5 mm提高到0.013 4 mm,最大預測誤差從0.059 7 mm減小到0.037 6 mm；通過兩者相結合，基于視控一體化系統，平均定位誤差在0.08 mm以內，良率96%，系統成本大大降低。因此，本文提出的機器人視覺飛拍系統具有簡捷易用、精度高和效率高優點，能夠滿足工業應用的需求。