機器人無標定手眼協調方法研究

盛榮 孫首群 焦玉格

摘 要：無標定情況下的視覺定位是機器人視覺伺服領域的熱點和難點。提出一種基于快速自適應濾波的機器人無標定手眼協調方法。首先根據機器人三維基坐標建立運動空間和圖像特征空間的微分映射關系，在不預先標定視覺傳感器與機器人參數情況下，設計一個直接估計增益矩陣[Kk]的自適應濾波器，在線辨識圖像雅可比矩陣。采用最小二乘法獲得狀態估計的初始值，在此基礎上設計視覺控制器和計算運動控制量。在MATLAB環境下建立機器人手眼協調控制系統仿真模型，將該方法用于觀測噪聲統計特性未知情況下的定位反饋，并對機器人空間螺旋運動的跟蹤情況進行分析。實驗結果表明，該自適應濾波器能夠完成不規則環境下視覺引導的追蹤任務，定位效果優于常規卡爾曼濾波和Sage-Husa自適應濾波，具有較強的魯棒性。

關鍵詞：視覺定位;無標定手眼協調;增益矩陣;在線辨識;自適應濾波器

DOI：10. 11907/rjdk. 191933 開放科學（資源服務）標識碼（OSID）：

中圖分類號：TP301文獻標識碼：A 文章編號：1672-7800（2020）005-0023-06

0 引言

無標定手眼協調是一個具有未建模動態的非線性系統控制問題，利用圖像雅可比矩陣模型描述機器人手眼映射關系是目前最為廣泛的一類方法。在機器人運動過程中，由于手眼關系的非線性，圖像雅可比矩陣變化十分明顯，能否在線獲得有效的圖像雅可比矩陣估計直接關乎機器人控制性能好壞。錢江等[1]提出了一種基于卡爾曼濾波算法的雅可比矩陣在線辨識策略，將雅可比矩陣的在線估計轉化為相應系統的狀態觀測問題。該算法缺點是需在噪聲方差陣[Q]和觀測噪聲方差陣[R]等模型參數已知的前提下進行;辛菁等[2]嘗試對系統噪聲進行實時估計，將Sage-Husa自適應卡爾曼濾波應用于在線辨識圖像雅可比矩陣，實現機器人在噪聲統計特性不完全已知情況下的視覺定位。但仿真實驗表明，該模型對[Q]和[R]同時估計會引起濾波發散;付夢印等[3]在有關導航系統研究中介紹了一種相關自適應濾波方法，利用觀測向量的相關性直接對增益矩陣[Kk]進行自適應估計。但這種方法涉及到非線性矩陣方程求解，計算復雜，實時性難以滿足要求。在過程噪聲和觀測噪聲為零均值高斯白噪聲前提下，Arasaratnam[4]提出了容積卡爾曼濾波（cubature Kalman filter，CKF）。該方法通過構造一組在個數、權系數和空間位置方面確定的“容積點”進行濾波，避免了Jacobian 矩陣求解，且無需可調參數;錢華明等[5]采用該方法進行大方位失準角初始對準，用降維算法優化了采樣點數，使計算時間減少了[1/3];程楊等[6]提出基于5階相徑容積規則的CKF算法，該算法采用多項式積分實現高斯積分近似;黃湘遠等[7]在此基礎上利用5階CKF對大失準角情況下的非線性初始對準工作進行研究，并通過平方根法增強濾波穩定性;Bhaumik[8]在CKF基礎上結合高斯—拉蓋爾積分準則，提出了容積求積卡爾曼濾波（cubature quadrature Kalman filter，CQKF）方法。該方法通過構造一組高階采樣點進行濾波，使濾波精度得到進一步提高，但此方法降低了濾波實時性;徐定杰等[9]運用最小化代價函數計算漸消因子，提升了噪聲干擾下的濾波精度，但是在估計誤差較大的情況下會導致濾波收斂緩慢，影響應用效率。為進一步實現機器人在噪聲統計特性未知情況下的實時視覺反饋和空間三維運動跟蹤，本文提出一種基于快速自適應濾波的機器人無標定手眼協調方法，改善了圖像雅可比矩陣辨識在不規則環境中的魯棒性和實時性，仿真結果表明該方法有效。

1 圖像雅克比矩陣在線辨識原理

定義機器人三維基坐標系為機器人的運動空間，機械人末端執行器在運動空間的坐標為[pg=（pgx pgy pgz）T]，而執行器在固定雙目攝像機的坐標系內，坐標為[pc1=（pc1x pc1y ][pc1z）T]和[pc2=（pc2x pc2y pc2z）T]，圖像特征空間即末端執行器在雙目攝像機中的圖像坐標為[fg=（fg1 fg2 fg3 fg4）]=[（ug1 vg1 ug2 ][vg2]）。圖像雅可比矩陣不是常定矩陣[10]，反映了機器人運動空間和圖像特征空間的微分映射關系，其定義如下：

圖像特征空間維數[m=4]，大于機器人運動空間維數[n=3]，可以唯一確定相應的空間位置。將手眼關系描述為圖像雅可比矩陣，實際上就是將一個靜態非線性系統近似為一個時變的線性系統，對雅可比矩陣的辨識就是對此線性時變系統參數的辨識[11]。

控制系統以待估的雅可比矩陣為系統狀態變量。使用卡爾曼濾波算法對系統狀態進行觀測，可以濾去外界噪聲對雅可比矩陣的干擾，使系統具有足夠的魯棒性，精度高、計算復雜度低 [12]，尤其在狀態估計[13]、目標跟蹤[14]、導航制導[15]、機械控制[16]等領域得到廣泛應用。實際情況中機器人的目標往往是隨機多變的，在未知動態的機器人視覺定位系統中，基于自適應卡爾曼濾波的圖像雅可比矩陣辨識模型主要存在兩種噪聲干擾：①由于機器人視覺系統成像模型發生改變而引發的過程噪聲[η（k）];②由于圖像采集與處理帶來的測量噪聲[v（k）][17]?？柭鼮V波算法在過程噪聲方差陣Q和測量噪聲方差陣R均未知的情況下，很難得到系統精確的數學模型和噪聲統計特性，對噪聲方差陣和測量噪聲方差陣同時估計會使濾波精度降低，甚至產生濾波發散現象，降低濾波器性能[18]。為此，本文繞過常規卡爾曼濾波必須已知系統噪聲方差陣[Q]和觀測噪聲方差陣[R]，設計一個直接估計增益矩陣[Kk]的自適應濾波器在線辨識圖像雅可比矩陣。

然后使用[j（0）]構成[x（0）]。在整個遞推估計過程中只需在系統初始時給出 3步試探運動，此后的雅可比矩陣將充分利用已完成的跟蹤運動信息進行自適應濾波遞推估計，無需再引入任何試探運動。

2 視覺控制器設計

全局視覺的目的是引導機器人末端大范圍跟蹤目標運動，建立最優反饋控制。

3 實驗與結果分析

為驗證本文提出的基于快速自適應濾波算法在機器人無標定手眼協調中的有效性，在MATLAB環境下建立機器人手眼協調模型。在隨機噪聲環境下，基于機器人空間視覺引導進行末端軌跡跟蹤實驗。

3.1 實驗設計

仿真條件：在MATLAB平臺上完成，使用機器人和機器視覺工具箱，建立機器人手眼協調控制系統仿真模型，圖1為控制系統結構。設雙目攝像機在機器人三維基坐標系中（3，0.5，0和3，0.5，0，單位為[m]）鏡頭分別與[Y]軸呈[π/12]和[-π/12]的夾角。雙目攝像機焦距[f1=f2=0.015m]，顯示的圖像平面分辨率為1 024×1 024，像素尺寸為[10-5×][10-5m/pixel]。

機器人采用工具箱中提供的PUMA560型六自由度關節式機器人，其連桿參數如表1所示，運動范圍為以肩部中心為球心、0.92[m]為半徑的空間半球。PUMA560型機器人的6個關節都是轉動關節，前3個關節確定手腕參考點位置，后3個關節用于確定手腕方位[20]。設置機器人末端[x、y、z]軸3個方向的最大移動速度均不超過[0.9m/s]。

在系統配置完全相同的情況下，動態目標作空間螺旋線運動，向雙目攝像機依次加入均值為0、最大幅值為[±3和±6]像素的隨機噪聲，然后比較常規卡爾曼濾波、Sege-Husa實時估計觀測噪聲濾波和快速自適應濾波為核心的3種手眼協調控制方法的跟蹤性能，以驗證本文提出的基于快速自適應濾波的機器人無標定手眼協調控制方法的有效性。

首先使用本文的手眼協調控制方法跟蹤上述空間螺旋線運動。圖2為MATLAB環境下的機器人手眼協調控制系統仿真模型（彩圖掃描OSID碼可見，下文同），圖3為機器人對直線運動的跟蹤情況，圖4顯示跟蹤運動完成時刻PUMA560機器人的位姿，圖中3個分散點為跟蹤開始前的3步試探運動，圖5為使用快速自適應濾波算法得到的機器人跟蹤軌跡，圖6和圖7是雙目攝像頭跟蹤過程中捕捉到的目標小球和機器人末端圖像坐標點集合。圖2-圖7顯示，基于快速自適應濾波的手眼協調控制方法能實現機器人對空間運動的跟蹤。

3.2 實驗結果

為進一步說明本文方法的跟蹤性能，在噪聲相同的情況下比較螺旋運動跟蹤過程中，3種濾波估計算法所形成的機器人末端與目標小球之間的空間距離變化。

圖8是隨機噪聲為[±3]像素、均值為0時，機器人分別使用3種濾波估計方法產生的跟蹤誤差變化曲線（綠色、藍色、紅色散點分別代表常規卡爾曼濾波、Sage-Husa自適應濾波、快速自適應濾波），在噪聲和攝像機參數完全相同的情況下，PUMA560機器人根據3種濾波算法估計的圖像雅可比矩陣進行跟蹤所產生的效果差距明顯。使用常規卡爾曼濾波接近目標（位置誤差趨于0）所用幀數最少，Sage-Husa自適應濾波算法次之，而快速自適應濾波算法開始跟蹤前需要收集12幀目標運動信息，故接近動態目標所用時間最長。表2是3種濾波算法的每幀平均濾波時長，常規卡爾曼濾波耗時最短，快速自適應濾波由于最初的12幀追蹤速度較慢，導致平均時長最長，是常規卡爾曼濾波的兩倍左右;Sage-Husa自適應濾波與常規卡爾曼濾波的平均耗時相近，都在3ms左右。在第50幀前，3種控制方法都完成了追趕動態目標任務。為了使之后的持續跟蹤產生顯著差異，本文選取[±6]像素、均值為0的隨機噪聲。下面比較3種濾波估計算法下，機器人相對目標曲線在[x、y、z]軸單一方向上的跟蹤情況，以及機器人跟蹤曲線與目標曲線空間位置上的誤差變化。

圖9至圖11顯示了在[±6]像素、均值為0的隨機噪聲下，3種濾波估計在單一方向上對動態目標的跟蹤情況。3種濾波估計都能有效追蹤目標，前50幀內3種估計方法誤差都比較大，常規卡爾曼濾波和Sage-Husa自適應濾波在第30幀前后誤差趨于穩定，快速自適應濾波在40幀后的追蹤軌跡與實際目標軌跡重合度較大（跟蹤誤差最?。?。圖12是3種濾波估計方法產生的跟蹤誤差變化曲線。常規卡爾曼濾波受固定的過程噪聲和觀測噪聲方差影響，跟蹤誤差變化幅值最大，動態目標每旋轉一周即沿軸線移動一個導程，跟蹤誤差就會達到一個峰值，最高峰值甚至超過了0.3m;Sage-Husa自適應濾波算法因為可以實時估計對系統影響較大的觀測噪聲，跟蹤誤差變化的幅度小于常規卡爾曼濾波，其在第200～250步之間的峰值最高，與常規卡爾曼濾波峰值相近，Sage-Husa自適應濾波算法的峰值平均在0.2m左右;使用快速自適應濾波算法的跟蹤過程顯然優于前兩者，50幀后的跟蹤誤差較為平穩，誤差的波峰與波谷變化幅度在0.1m以內，3個導程范圍內產生了6個峰值，最高峰值低于0.2m，平均峰值0.15m左右。

4 結語

本文針對機器人在無標定手眼協調中對視覺反饋較高的魯棒性要求，提出一種基于快速自適應濾波的機器人無標定手眼協調方法。該方法通過建立自適應卡爾曼濾波遞推估計和最優反饋控制，設計一個直接估計增益矩陣[Kk]的自適應濾波器在線辨識圖像雅可比矩陣，實現了對噪聲統計特性未知情況下的實時視覺反饋和空間三維運動跟蹤。通過MATLABR實驗平臺進行仿真實驗，對機器人無標定手眼協調下的定位方法研究成果如下：

（1）快速自適應濾波算法能實現對圖像雅克比矩陣的在線辨識，無需預先知道噪聲的統計特性和視覺傳感器詳細參數，且可忽略噪聲模型誤差帶來的影響，對提高圖像雅克比矩陣的辨識精度有重要意義。

（2）仿真結果表明本文算法濾波精度優于常規卡爾曼濾波和Sage-Husa自適應濾波。由于快速自適應濾波需要收集12幀目標運動信息，所以接近動態目標所用時間最長。但是在第50幀之后，3種濾波算法誤差趨于穩定，不影響整體的濾波估計。結果表明本文設計的快速自適應濾波器可在噪聲特性未知的情況下提高視覺反饋系統魯棒性。

本文的不足之處是未詳細討論快速自適應濾波算法濾波速度較慢的原因，關于前12幀信息收集時間對整個濾波過程的影響情況未深入探討。

參考文獻：

[1] 錢江. 基于圖象雅克比矩陣的無標定手眼協調[D]. 上海：上海交通大學， 2003.

[2] 辛菁，白蕾，劉丁. 基于自適應Kalman濾波的機器人6DOF無標定視覺定位[J]. 系統仿真學報，2014，26（3）：586-591.

[3] 付夢印，肖烜，鄧志紅，等. 陸用慣性導航技術[M]. 北京：科學出版社，2017：1-63.

[4] ARASARATNAM I，HAYKIN S. Cubature kalman filters[J]. IEEE Transactions on Automatic Control，2009，54（6）：1254-1269.

[5] 錢華明，黃蔚，孫龍. 基于改進的迭代容積卡爾曼濾波姿態估計[J]. 哈爾濱工業大學學報，2014，46（6）：116-122.

[6] BHAUMIK S， SWATI. Cubature quadrature kalman filter[J]. IET Signal Process，2013，7（7）： 533-541.

[7] JIA B，XIN M，CHENG Y. High-degree cubature kalman filter[J]. Automatica，2013，49（2）：510-518

[8] 黃湘遠，湯霞清，武萌. 5階CFK在捷聯慣導非線性對準中的應用研究[J]. 系統工程與電子技術，2015，37（3）：633-638.

[9] 徐定杰，沈忱，沈峰. 時變有色觀測噪聲下基于變分貝葉斯學的自適應卡爾曼濾波[J]. 電子與信息學報，2013，35（7）：1593-1598.

[10] 徐波，王金生，徐洲石. 智能巡檢機器人調度與路線規劃能力[J]. 長春工業大學學報，2016，37（3）：269-272.

[11] 秦偉洋，岳曉峰，吳煥新，等. 機器人無標定手眼協調系統視覺跟蹤[J]. 長春工業大學學報，2018，39（3）：233-237.

[12] DUNIK J，SIMANDL M，STRAKA O. Unscented kalman filter： aspects and adaptive setting of scaling parameter [J]. IEEE Transactions on Automatic Control，2012，57（9）：2411-2416.

[13] GUSTAFDDON F， HENDEBY G. Some relations between extended and unscented and unscented Kalman filters[J]. IEEE Transactions on Signal Processing，2012，60（2）：545-555.

[14] 蔣恩松，李孟超，孫劉杰. 一種基于神經網絡的卡爾曼濾波改進方法[J]. 電子與信息學報，2007，29（9）：2073-2076.

[15] 徐定杰，賀銳，沈峰，等. 基于新息協方差的自適應漸消卡爾曼濾波器[J]. 系統工程與電子技術，2014，36（7）：1405-1409

[16] 李理敏，龔文斌，劉會杰. 基于自適應擴展卡爾曼濾波的載波跟蹤算法[J]. 航空學報，2013，33（7）：1319-1328.

[17] 周飛菲. 噪聲統計特性失配的魯棒目標跟蹤方法[J]. 電子測量與儀器學報，2018，32（9）：195-201.

[18] ARULAMPALAM M S，MASKELL S，GORDON N，et al. A tutorial on particle filters for online nonlinear/non-Gaussian Bayesian tracking[J]. IEEE Transactions on Signal Processing，2002，50（2）：174-188.

[19] 張憲霞. 基于SPSA優化的Kalman濾波無標定視覺伺服[J]. 系統仿真學報，2018，30（12）：4754-4759.

[20] 李輝，李開世，黃文權. 基于MATLAB與ADAMS的PUMA560機器人逆運動仿真[J]. 機械工程師，2014（11）：105-106.

（責任編輯：杜能鋼）