基于自適應無跡卡爾曼濾波的采摘機械手視覺伺服控制方法

梁喜鳳，彭明，路杰，秦超

（中國計量大學機電工程學院，杭州310018）

0 引 言

視覺伺服技術是機器人的關鍵技術，在采摘機器人中應用也越來越廣泛[1-2]。圖像雅克比矩陣將視覺空間信息反饋到機械手運動空間，是基于圖像的無標定視覺伺服控制方法的核心內容，其估計精度直接影響系統的視覺伺服控制精度[3]。對于圖像雅克比矩陣的在線估計，國內外學者已經進行了大量研究。Yoshi和HOSODA 等提出了使用最小二乘法在線估計圖像雅克比矩陣[4-5]。Piepmeier 提出了動態擬牛頓法估計圖像雅克比矩陣，實現無標定手眼協調[6-7]。Matthew 提出了使用卡爾曼濾波以及自適應卡爾曼濾波的方法估計圖像雅克比矩陣，實現對靜態目標的定位和動態目標的跟蹤[8-10]。Sang 等將魯棒控制器與卡爾曼濾波方法結合起來形成魯棒卡爾曼濾波，從而實現圖像雅克比矩陣的在線估計[11-12]。哈爾濱工業大學機器人研究所李牧提出“眼在手”無標定機器人視覺伺服控制算法，通過將非線性目標函數最小化,采用迭代最小二乘法提高系統的穩定性，以視覺信息跟蹤動態圖像[13-14]。趙清杰等研究了基于粒子濾波估計圖像雅克比矩陣的方法，實驗證明該方法比卡爾曼濾波具有更高的估計精度和更強的魯棒性[15-16]。辛菁等在噪聲的統計特性不完全已知的情況下，設計自適應Kalman 濾波器能使6 自由度機器人到達期望的位置，且定位精度高[17-18]。孫冬雪提出了自適應卡爾曼濾波算法估計圖像雅克比矩陣，進行過程噪聲和測量噪聲的在線估計和調節，補償了圖像采集、傳輸和處理帶來的延時[19]。以上圖像雅克比矩陣估計方法均以卡爾曼濾波法為基礎，將機器人手眼關系近似線性化，且在視覺伺服過程中將時變的系統噪聲協方差矩陣設為常數矩陣[20-22]。在系統噪聲協方差矩陣選取恰當的情況下，上述方法均能很好地反映機器人手眼映射關系，實現視覺伺服任務。然而在實際視覺伺服過程中，系統噪聲矩陣會發生變化，從而會影響視覺伺服控制精度，增加視覺伺服過程所用時間。

為克服未知統計特性的系統噪聲對采摘機械手圖像雅克比矩陣估計精度的影響，提高視覺伺服控制精度，減少視覺伺服過程所用時間，本文在KF 算法以及UKF 算法估計圖像雅克比矩陣的基礎上，對系統過程噪聲協方差矩陣引入自適應噪聲統計估計器，還原非線性的手眼映射關系，提出AUKF 算法，利用該算法估計圖像雅可比矩陣并構建視覺伺服控制系統，以7 自由度番茄串采摘機械手為對象搭建試驗平臺，進行AUKF 算法可行性驗證。

1 圖像雅克比矩陣

基于圖像的視覺伺服控制方法是通過相機采集目標，經過圖像處理后提取目標圖像特征，與機械手末端期望圖像特征進行比較，經過圖像雅克比矩陣轉化為機械手末端空間位姿變化量。機械手末端位姿變化量再經過機械手雅克比矩陣，轉化為各關節變化量，通過積分得到確定的關節角度增量，從而驅動機械手向固定目標運動。圖像雅克比矩陣描述高度非線性的機器人手眼關系，即圖像特征變化量與機械手末端位姿變化量之間的關系。利用圖像雅克比矩陣的反饋信息，映射到機器人控制系

統中，可對機器人運動進行控制。其數學模型可表示為

2 基于AUKF算法估計圖像雅克比矩陣

在傳統KF 算法以及UKF 算法估計圖像雅可比矩陣過程[20-22]中，將過程噪聲方差矩陣Qk和系統噪聲qk設為常數矩陣，并且在視覺伺服過程中保持不變。為適應時變的系統過程噪聲統計特性，本文利用系統狀態一步預測值與狀態更新值之差構造噪聲估計遞推器，對UKF 算法中的系統噪聲qk以及系統噪聲協方差矩陣Qk進行自適應估計，構造AUKF 算法。定義圖像雅克比矩陣的觀測量作為系統狀態輸入，機械手末端運動所引起的圖像特征變化為系統輸出，設置濾波次數為100，AUKF 算法估計圖像雅可比矩陣流程如圖1所示。qk和Qk由自適應噪聲統計估計器遞推獲得

圖1 AUKF算法估計圖像雅克比矩陣流程圖Fig.1 Flow chart of estimating image Jacobian matrix with AUKF

在圖像雅可比矩陣估計方法中，KF法的時間復雜度為O( N×n2)，空間復雜度為O(n3)。UKF 法使用UT 變換求取一步預測值，其時間復雜度為O(2M×N×n2)，空間復雜度為O(n3)。AUKF法在UKF法的基礎上引入了自適應噪聲估計器，并為qk與Qk額外分配存儲空間，故其時間復雜度為O(2M×N×2n2)，空間復雜度大于O(n3)。其中M為Sigma點個數，N為總采樣次數，n為預測協方差陣維數。因此，AUKF 法的時間復雜度與空間復雜度均高于傳統KF法與UKF法。

3 采摘機械手視覺控制器模型

3.1 圖像特征選取

本文機械手的采摘對象為果實串，與單個果實采摘僅以果實質心的圖像特征作為視覺伺服系統輸入信息不同[23-25]，果實串采摘時夾持與切割目標點均在果梗上，因此選取機械手末端執行器上2 點即切割點與夾持點作為期望特征點，果梗上2 點即采摘點與夾持點作為目標特征點。利用相機參數矩陣將目標特征點與期望特征點在相機坐標系下的三維坐標變換為像素坐標系下的坐標，變換過程為

其中ρw,ρh為每個像素點各自的寬和高，(u0,v0)為圖像平面與光軸的交點，為空間點P 在相機坐標系下的坐標為空間點P 在像素坐標系中的坐標。目標點在像素平面成像坐標如圖2所示。

圖2 期望特征與目標特征像素坐標Fig.2 Desired feature and target feature pixel coordinates

圖2 中，選取期望特征點與目標特征點像素坐標差u1,v1,u2,v2,u3,u4作為圖像特征誤差Δf，6 個圖像特征誤差計算方法為

式中u1為左目相機中夾持點與被夾持點橫坐標差，像素；v1為左、右相機中夾持點與被夾持點縱坐標差。u2為左目相機中夾持點與被夾持點橫坐標差；v2為左、右相機中切割點與被切割點縱坐標差；u3為左目相機中切割點與被切割點橫坐標差；u4為右目相機中切割點與被切割點橫坐標差。

視覺伺服的任務是使機械手末端2 點與果梗上2 個目標點重合，即圖像特征誤差趨近于0。

3.2 視覺控制器模型

視覺控制器模型將圖像特征變化速度通過圖像雅克比矩陣轉化為機械手末端三維空間運動速度[26]，輸入圖像特征變化速度，輸出機械手末端空間運動速度，其表達式為

其中J-1為圖像雅克比矩陣逆。

期望圖像特征fd為機械手末端夾持點與采摘點在相機成像平面的坐標，實際圖像特征f為目標點在成像平面的坐標，Δe 為二者特征之差。本文利用P 控制方法建立視覺控制器模型，即：

4 仿真試驗

4.1 機械手視覺伺服仿真系統

基于AUKF 算法設計視覺伺服控制系統，通過Robotic Toolbox 工具箱以及Machine Visual Toolbox 工具箱建立機械手模型與相機模型，利用Simulink 模塊構建視覺伺服控制系統。視覺伺服控制流程如圖3所示。

圖3 視覺伺服控制流程圖Fig.3 Flow chart of visual servo control system

視覺伺服系統根據輸入的實際圖像特征估計圖像雅可比矩陣J(r)，經過求逆后得到圖像雅可比矩陣的逆。期望圖像特征為末端執行器上切割點與夾持點在相機平面內的成像坐標，在視覺伺服過程中為定值。實際圖像特征與期望圖像特征的差值為圖像特征誤差，經過計算后將圖像特征變化量轉化為機械手末端空間速率，其中k 為比例系數，經多次試驗論證后，取最佳值為0.1。J(q)經過求偽逆后，與機械手末端空間速率相乘得出關節角速率，即q·= J+(q)· r·。經過積分器運算后得到Δq 作為正運動學的輸入，輸出為末端期望空間坐標P。同時q又輸入到機械手雅可比矩陣模塊，更新關節角度，即在下一次視覺伺服過程中，關節角度為(q + Δq )，在視覺伺服時間內，該循環始終存在，循環判斷條件為是否達到設置的仿真時間。

4.2 仿真參數設置

為驗證AUKF 算法估計圖像雅克比矩陣的可行性，進行視覺伺服控制系統仿真試驗，使機械手在視覺伺服系統控制下，從初始位置向目標點運動。仿真試驗中，雙目相機模型焦距均為0.008 m，像素尺寸均為（1e-0.5,1e-0.5）(單位:m/像素)，圖像橫軸和縱軸的分辨率均為1 280和1 024，(單位:像素), 圖像中心像素坐標為（512，512）( 單 位: 像 素)。 機 械 手 關 節 角 度 初 始 值 為q =[0,0.4,0,0,0,pi/2,0 ]，在機械手初始狀態下，空間中點P1(1.055,0.4,0.15)、P2(1.05,0.45,0.2)位于相機視野范圍內，故取P1、P2兩點作為目標點（果梗上被夾持點與被切割點）。目標點在相機中的初始圖像特征誤差為[u1,v1,u2,v2,u3,u4]=[110,120,100,50,45,20](單 位:pixels)。仿真時間設置為10 s，使用變步長求解器，求解器類型為4 階龍格-庫塔。視覺伺服過程結束后，在系統輸出模塊讀取最終圖像特征誤差、末端執行器與目標位置誤差以及響應時間。

4.3 仿真結果與分析

基于KF 法、UKF 算法、AUKF 算法估計圖像雅克比矩陣構建的視覺伺服控制系統圖像特征誤差仿真圖像以及最終圖像特征誤差如圖4所示。

由圖4 可知，機械手末端上的特征點與目標特征點在像素平面上的坐標差呈衰減趨勢，最終實現機械手末端向目標靠近。由表1 可知，基于AUKF 算法的圖像特征誤差平均值為4.978 像素，相比于KF 算法（11.4985 像素）與UKF 算法（6.6175 像素），分別減小了54.44%與24.78%?；贏UKF 算法的視覺伺服控制系統仿真結果的6 個圖像特征誤差值均小于KF 算法和UKF 算法的視覺伺服控制系統。

機械手末端夾持點與目標三維坐標差初始值：[ΔX,ΔY,ΔZ] =[0.01,-1.45,0.75](單位:m)，視覺伺服控制系統運行過程中機械手末端執行器上夾持點與目標點的位置誤差變化如圖5示。

圖4 圖像特征誤差變化過程Fig.4 Changing progress of image feature errors

圖5 末端執行器位置與目標位置誤差Fig.5 Changing progress errors between end-effector position and target position

由圖5 可知，在視覺伺服過程中，末端執行器與目標點之間的三維坐標差呈衰減趨勢。由表2 可知，基于AUKF 算法的視覺伺服控制系統中，末端執行器最終位置與目標距離為4.19 mm?；贙F 算法和UKF 算法的視覺伺服控制系統，末端執行器最終位置與目標距離誤差分別為20.68 和21.33 mm，AUKF 算法相比于KF 算法與UKF 法構建的視覺伺服控制系統距離誤差分別減小了79.74% 與80.36%。基于KF 算法、UKF 算法以及AUKF 算法的視覺伺服控制系統響應時間分別為2 s、1.5 s 以及1.2 s，AUKF 算法相比于KF 算法與UKF 算法，響應時間分別減少了40%與20%。結果表明，采用AUKF 算法估計的圖像雅克比矩陣，能夠克服未知統計特性的系統過程噪聲，使圖像雅克比矩陣估計更加準確，響應時間更短。

表1 圖像特征誤差仿真結果Table1 Simulation results of image features error pixels

表2 位置誤差仿真結果Table2 Simulation results of position errors mm

5 驗證試驗

5.1 機械手視覺伺服系統試驗平臺

為驗證基于AUKF圖像雅可比矩陣估計算法的視覺伺服控制系統在實際采摘中的可行性，評價視覺伺服控制精度，搭建了7 自由度番茄串采摘機械手視覺伺服系統試驗平臺，如圖6所示。

圖6 采摘機械手視覺伺服系統試驗平臺Fig.6 Picking manipulator visual servo system test platform

視覺伺服系統以PC 機作為上位機，操作系統為Win7(64bit)，處理器為Intel Core i5-4200M 2.5GHz。雙目相機為COMS 彩色相機，型號為DC-USB300-B，靈敏度為1.0V/lux-sec550 nm，光譜響應范圍為400～1030 nm，像素尺寸為3.2 μm×3.2 μm，動態范圍大于61 db，信噪比為43 db，采用650 nm 低通濾光片。相機像素設置為640+480 像素，采用USB 數據線與上位機通信。上位機開發采用軟觸發方式控制相機采集圖像，相機自定義數據為256 字節塊數據區域。雙目相機平行放置于末端執行器兩側，間距為15 cm。下位機是以ARM 公司生產的STM32F103系列芯片為核心的微控制器，通過單脈沖模式控制步進電機驅動器。

5.2 試驗設計

5.2.1 試驗指標

1）采摘成功率

采摘成功率計算如下：

其中η 為采摘成功率，n 為試驗成功次數，M 為總試驗次數，30次。

2）圖像特征誤差平均值

在相機成像平面內，讀取視覺伺服結束后的被切割點與被夾持的成像坐標，與末端執行器上的切割點與夾持點成像坐標作差得到圖像特征誤差。所有采摘成功試驗的圖像特征誤差平均值為

3）單次平均采摘時間

視覺伺服過程中，末端執行器與目標距離誤差不斷減小，當果梗位于末端執行器上夾爪夾持范圍內時認為到達采摘點位置，視覺伺服過程結束，試驗用夾爪夾持范圍為17.7、16.0和28.0 mm。利用計時器記錄視覺伺服過程開始到結束所有時間，即為單次采摘時間，統計所有采摘成功試驗的采摘時間平均值為：

5.2.2 試驗方案

分別基于KF、UKF、AUKF 圖像雅可比矩陣估計算法的視覺系統進行靜態與動態采摘試驗。 靜態采摘試驗時，將番茄果實串懸掛于移動平臺并保持靜止不動，機械手在相對于目標的任意初始位置開始向目標點運動實現采摘。動態采摘試驗時，控制移動平臺做小范圍的往復擺動，使番茄串產生不確定的振動。移動平臺電機驅動器位于移動平臺絲桿末端，電機驅動器型號為HBS57，頻率為1 500 Hz，移動范圍為-0.02～0.02 m。

每種方法的靜態與動態采摘試驗各30 次，每次試驗均保持機械手初始位姿相同，統計靜態與動態條件下采摘成功率、圖像特征誤差平均值以及單次平均采摘時間。

5.3 試驗流程

利用MATLAB 編寫的上位機控制界面對雙目相機采集到的圖像進行處理以及目標特征提取。為將果梗從復雜的背景中提取出來，利用支持向量機法分離出目標果梗圖像[27-28]。分離出的果梗圖像經過灰度化、閾值分割、形態學開運算等處理，得到番茄串果梗的二值圖像。通過連通組件標記算法找到所有的連通區域，并分別標記，對每個連通區域運用計算幾何距算法得到質心。將果梗質心點作為被夾持點，根據番茄串垂直生長特性以及末端上夾持點與切割點在圖像上的距離，提取被夾持點上方一點作為被切割點。由于雙目相機固定于機械手末端，故夾持點與切割點圖像坐標為定值，在成像平面上垂直距離為80 像素，試驗前預先標定好。通過視覺伺服控制算法將目標在相機平面的圖像特征變化量轉化為關節角度變化量。上位機將關節角度變化量對應的脈沖數與頻率通過串口傳送到下位機。步進電機驅動器把控制器發出的脈沖信號轉化為步進電機的角位移，從而驅動機械手關節運動，具體試驗流程如圖7 所示。

圖7試驗過程流程圖Fig.7 Flow chart of experiment progress

5.4 采摘試驗結果與分析

視覺伺服過程結束后，基于KF、UKF 與AUKF 3 種圖像雅克比矩陣估計算法的視覺伺服系統采摘失敗次數與成功率如表3所示。

表3 采摘試驗成功率Table 3 Success rate of picking experiment

從表3 可知，基于AUKF 算法的視覺伺服系統靜態采摘試驗成功率為90%，動態采摘試驗成功率為83%，相比于KF 算法與UKF 算法，靜態采摘試驗成功率分別提高了17 與10 個百分點，動態采摘試驗成功率分別提高了16 與10 個百分點。采摘試驗失敗的原因是圖像雅克比矩陣估計不準確，導致機械手末端執行器運動到相機視野之外。除去試驗失敗的數據，基于3 種圖像雅克比矩陣估計方法的視覺伺服系統最終圖像特征誤差平均值如表4 所示。

表4 圖像特征誤差平均值Table 4 Average value of image feature error pixels

由表4 可知，基于AUKF 算法的視覺伺服系統靜態采摘的最終圖像特征誤差平均值相比于基于KF 算法與UKF 算法估計圖像雅克比矩陣的視覺伺服系統分別減小了70.39%，80.20%，68.89%，80.20%，70.39%，68.89%與49.84%，67.84%，51.56%，67.84%，49.84%，51.56%，動態采摘的最終圖像特征誤差平均值相比于基于KF 算法與UKF 算法的視覺伺服系統分別減小了50.94%，54.53%，74.32%，54.53%，50.94%，74.32% 與30.42%，37.14%，50.19%，37.14%，30.42%，50.19%。

3 種圖像雅克比矩陣估計方法的視覺伺服系統響應時間如表5所示。

表5 單次采摘平均時間Table 5 Average value of picking time s

由表5可知，基于AUKF算法的視覺伺服系統在目標靜態條件和動態條件平均用時分別為18和22 s，相比于KF算法與UKF算法，目標靜態條件下用時分別減小了10與6 s，減小的百分比為35.71%與25%。目標動態條件下用時分別減小了12與8 s，減小的百分比為35.29%與26.67%。

6 結 論

1）本文在KF 算法與UKF 算法估計圖像雅克比矩陣的基礎上，引入自適應系統噪聲估計器，提出的AUKF算法估計圖像雅克比矩陣，能夠實現時變過程噪聲適應系統的變化，使圖像雅克比矩陣估計更加準確。

2）視覺伺服控制系統仿真試驗結果表明，在系統噪聲統計特性未知的情況下，基于AUKF 算法的視覺伺服控制系統圖像特征誤差平均值為4.978 像素，相比于KF算法與UKF 算法，分別減小了54.44%與24.78%；機械手末端與目標最終距離為4.19 mm，相比于KF 算法與UKF 算法的視覺伺服控制系統分別減小了79.74%與80.36%。因此AUKF 算法能抑制未知統計特性的系統過程噪聲對圖像雅克比矩陣估計精度的影響。

3）番茄串采摘機械手靜態采摘試驗時，基于AUKF算法的視覺伺服系統與基于KF 算法和UKF 算法視覺伺服系統相比，最終圖像特征誤差平均值分別減小了68.89%～80.20%和49.84%～67.84%，采摘時間分別減小了35.71%與25%；動態采摘時，最終圖像特征誤差平均值分別減小了50.94%～74.32%和30.42%～50.19%，采摘時間分別減少了35.29%與26.67%。基于AUKF 算法的視覺伺服系統靜態和動態采摘試驗成功率分別為90%和83%，相比于KF 算法與UKF 算法，分別提高了17 與10和16與10個百分點。