卡爾曼濾波與模糊邏輯結(jié)合的機(jī)械手視覺伺服控制方法研究

王亞威

(中國船舶重工集團(tuán)公司第713研究所，河南鄭州450015）

1 引言

隨著機(jī)器人技術(shù)的迅猛發(fā)展，機(jī)器人承擔(dān)的任務(wù)更加復(fù)雜多樣。但是，早期機(jī)器人大多是示教型機(jī)器人，可成功的抓取一個(gè)方向、位置已知的物體，但當(dāng)物體位置和方向變化時(shí)，常常導(dǎo)致抓取操作的失敗，這是因?yàn)檫@些系統(tǒng)中缺乏視覺反饋的緣故。利用從圖像中提取的視覺信息，進(jìn)行機(jī)器人末端執(zhí)行器的位置閉環(huán)控制的系統(tǒng)稱為視覺伺服系統(tǒng)[1]。視覺伺服控制方法不僅能夠獲取足夠的外部環(huán)境信息，還能避免與外部環(huán)境直接接觸，提高了機(jī)器人系統(tǒng)的靈活性和生存能力，已在無人機(jī)目標(biāo)跟蹤及自主降落、虛擬現(xiàn)實(shí)、水下機(jī)器人導(dǎo)航等領(lǐng)域得到廣泛應(yīng)用。

視覺伺服控制方法基于機(jī)器視覺原理對視覺傳感器獲取的圖像信息進(jìn)行分析處理得到相應(yīng)的數(shù)據(jù)，利用這些圖像數(shù)據(jù)設(shè)計(jì)控制算法，將控制信息提供給機(jī)器人，構(gòu)造機(jī)器人的位置閉環(huán)控制。傳統(tǒng)的基于圖像的視覺伺服控制方法的研究主要集中在圖像雅可比矩陣、圖像雅可比矩陣的穆爾彭羅斯廣義逆矩陣的精確獲取以及對控制器系數(shù)的選取[2]。圖像雅可比矩陣定義了現(xiàn)實(shí)世界中三維點(diǎn)與圖像二維平面投影點(diǎn)的速度間的轉(zhuǎn)換關(guān)系，由相機(jī)固有參數(shù)以及其在現(xiàn)實(shí)世界中三維點(diǎn)的深度信息獲取[3]。然而，相機(jī)的固有參數(shù)通常是未知的或不準(zhǔn)確的，傳統(tǒng)的視覺伺服控制在求解圖像雅可比矩陣前需要花費(fèi)大量的時(shí)間進(jìn)行相機(jī)標(biāo)定實(shí)驗(yàn)[4]來獲取準(zhǔn)確的相機(jī)參數(shù)，降低了視覺伺服控制方法的實(shí)用性。為了解決這一難題，研究人員開始致力于研究無需獲知相機(jī)參數(shù)來計(jì)算圖像雅克比矩陣的視覺伺服控制方法。鄭立斌等運(yùn)用RBF神經(jīng)網(wǎng)絡(luò)有效逼近圖像差值和關(guān)節(jié)角變化之間的非線性映射關(guān)系，簡化了傳統(tǒng)的視覺伺服控制方法[5]。王博等提出了一種動(dòng)態(tài)擬牛頓算法模擬逼近圖像雅克比矩陣，使機(jī)器人的智能抓取免去了復(fù)雜的標(biāo)定程序，克服了原視覺系統(tǒng)的不確定因素，提高了機(jī)器人視覺系統(tǒng)的魯棒性[6-8]。趙航等提出了一種基于PSO-GA-BP神經(jīng)網(wǎng)絡(luò)視覺伺服控制方法極大地簡化了計(jì)算的復(fù)雜程度，準(zhǔn)確地建立了圖像特征變化和關(guān)節(jié)角變化間的非線性關(guān)系[9]。Gao 等設(shè)計(jì)了自適應(yīng)神經(jīng)網(wǎng)絡(luò)控制器，利用估計(jì)的全局位姿信息對水下機(jī)器人進(jìn)行跟蹤，保證了跟蹤誤差的收斂性[10]。通過研究國內(nèi)外學(xué)者提出的方法發(fā)現(xiàn)，目前使用的智能控制方法存在模型訓(xùn)練時(shí)間長、控制系統(tǒng)收斂速度慢等弊端。本文結(jié)合前人的基礎(chǔ)，提出了一種卡爾曼濾波與模糊邏輯控制算法相結(jié)合的視覺伺服控制方法，既避免了長時(shí)間的模型訓(xùn)練又提高了控制系統(tǒng)的收斂速度。并通過MATLAB仿真檢驗(yàn)了提出的控制方法的有效性與優(yōu)越性。

2 視覺伺服控制器的獲取

目前，國內(nèi)外針對基于圖像的視覺伺服控制方法的研究主要集中在對視覺伺服控制器系數(shù)的選取或者圖像雅可比矩陣的精確獲取上。

視覺伺服控制的目標(biāo)為n 個(gè)特征點(diǎn)矢量坐標(biāo)與其理想特征點(diǎn)矢量坐標(biāo)誤差的最小化：

本文研究的機(jī)械手視覺伺服控制系統(tǒng)為相機(jī)固定在6 自由度機(jī)械手末端執(zhí)行器上構(gòu)成的單目眼在手構(gòu)型。機(jī)械手末端執(zhí)行器速度與圖像特征像素坐標(biāo)變化具有如下關(guān)系[11]：

上式中v=(vx，vy，vz，wx，wy，wz)T為6 自由度機(jī)械手末端執(zhí)行器的速度，vx，vy，vz為線速度，wx，wy，wz為角速度。Jp為n個(gè)特征點(diǎn)的圖像雅克比矩陣，具有如下形式：

式中kx，ky，u0，v0為相機(jī)參數(shù)，Z為相機(jī)深度。其中，相機(jī)參數(shù)為未知的常量，需要相機(jī)標(biāo)定獲取。在基于圖像的機(jī)械手視覺伺服控制中，相機(jī)深度Z的值對圖像雅可比矩陣影響很小，通常取常量[10]。

結(jié)合公式(1)與公式(2)可得圖像特征誤差與機(jī)械手末端執(zhí)行器速度之間的關(guān)系：

為了使圖像特征誤差快速減小，通常有如下公式成立：

結(jié)合(3)與(4)可得傳統(tǒng)的視覺伺服控制器形式如下：

公式(5)中J+p為Jp的穆爾彭羅斯廣義逆矩陣，λ為非負(fù)的常數(shù)。

使用公式(5)所表示的視覺伺服控制器構(gòu)建視覺伺服控制系統(tǒng)，將速度控制指令v輸入到機(jī)械手控制器，控制機(jī)械手完成定位任務(wù)。

3 圖像雅可比矩陣Jp的在線辨識

圖像雅可比矩陣用來表示機(jī)器人末端執(zhí)行器的位姿變化率與圖像特征空間變化率的微分映射關(guān)系，實(shí)際上就是將系統(tǒng)近似成為一個(gè)時(shí)變的線性系統(tǒng)，對圖像雅可比矩陣的估計(jì)就轉(zhuǎn)變?yōu)閷υ摼€性系統(tǒng)參數(shù)的估計(jì)[12]。如果所構(gòu)造的系統(tǒng)以圖像雅可比矩陣為狀態(tài)變量，就能夠使用卡爾曼濾波方法對圖像雅可比矩陣進(jìn)行辨識[13]。

圖像雅可比矩陣表征了機(jī)械手末端執(zhí)行器位姿變化與圖像特征坐標(biāo)變化之間的關(guān)系，具有如下形式：

式中p∈Rn表示末端執(zhí)行器的位姿，f∈Rm表示圖像特征坐標(biāo)。

將公式(6)離散化得到：

為實(shí)現(xiàn)對J(p)的估計(jì)，定義mn×1維觀測向量為：

其中：

為圖像雅可比矩陣的第i行向量。

令圖像雅可比矩陣x(k)為系統(tǒng)狀態(tài)變量，狀態(tài)轉(zhuǎn)移矩陣選取為單位矩陣；并且令圖像特征的改變△f(k)=f(k+1)-f(k)為系統(tǒng)的輸出即y(k)=△f(k)。

卡爾曼濾波是一種獨(dú)立高斯白噪聲環(huán)境下的最優(yōu)線性狀態(tài)估計(jì)算法。所以可以將待估計(jì)的矩陣元素作為卡爾曼濾波系統(tǒng)的狀態(tài)變量，從而將圖像雅克比矩陣的估計(jì)問題轉(zhuǎn)化為對系統(tǒng)狀態(tài)的觀測問題[14]。其狀態(tài)模型和觀測模型如下：

式中φ(k)為過程噪聲、v(k)為測量噪聲。

則按卡爾曼濾波建立遞推估計(jì)[15]：

式中Q為過程噪聲協(xié)方差且R為觀測噪聲協(xié)方差，Pk|k為狀態(tài)估計(jì)誤差協(xié)方差。公式(10)中的5 個(gè)卡爾曼濾波遞推估計(jì)公式是一個(gè)遞歸循環(huán)過程，包含預(yù)測和更新兩個(gè)步驟，前2個(gè)公式為對狀態(tài)變量和狀態(tài)估計(jì)誤差協(xié)方差的預(yù)測估計(jì)，后3 個(gè)公式為對卡爾曼增益、狀態(tài)變量以及狀態(tài)估計(jì)誤差協(xié)方差的修正。當(dāng)給定動(dòng)態(tài)模型的初始化狀態(tài)X(0)、過程噪聲協(xié)方差Q、觀測噪聲協(xié)方差R及狀態(tài)估計(jì)誤差協(xié)方差的初始值P(0)等參數(shù)后，就可通過卡爾曼濾波的循環(huán)運(yùn)算實(shí)現(xiàn)對當(dāng)前時(shí)刻狀態(tài)變量(圖像雅可比矩陣)的在線辨識。

綜上，使用模糊邏輯獲取比例控制系數(shù)，使用卡爾曼濾波方法在線估計(jì)圖像雅可比矩陣，構(gòu)建視覺伺服控制系統(tǒng)如圖1。

圖1 視覺伺服控制系統(tǒng)

4 基于模糊邏輯控制器的λ值獲取

傳統(tǒng)的視覺伺服控制方法中，λ值選取以后就無法改變。如果λ選取偏大，雖然能夠提高控制系統(tǒng)的收斂速度，但是可能會(huì)導(dǎo)致機(jī)械手速度過大。相反，如果λ選取偏小，會(huì)使控制系統(tǒng)收斂速度變慢。另外，利用卡爾曼濾波算法獲取圖像雅克比矩陣的同時(shí)引入了噪聲干擾，這種干擾可能會(huì)導(dǎo)致伺服系統(tǒng)收斂較慢，收斂性能較差。為了克服以上缺點(diǎn)，使用模糊邏輯的思想設(shè)計(jì)了一種模糊控制器，能夠使控制器系數(shù)λ的值隨圖像誤差自適應(yīng)的變化。當(dāng)圖像誤差較大時(shí)，選取較小的λ防止機(jī)械手速度過大，當(dāng)圖像誤差較小時(shí)選取較大的λ加快控制系統(tǒng)的收斂速度。

本文選取圖像坐標(biāo)差值e的范數(shù)以及e的變化率 的范數(shù)為輸入，λ為輸出。對||e||、||||和λ分別定義三個(gè)隸屬度函數(shù)(低、中、高)，隸屬度函數(shù)均選擇高斯曲線。建立模糊規(guī)則庫如表1，L、M、H分別表示低、中、高，所有規(guī)則采用“與”操作符[16]。

表1 模糊規(guī)則

建立模糊隸屬度曲線如圖2。

圖2 輸入輸出隸屬度函數(shù)

根據(jù)模糊規(guī)則可以得到輸入||e||和||||與輸出λ的關(guān)系如圖3所示。

圖3 輸入輸出關(guān)系

5 基于模糊邏輯與卡爾曼濾波相結(jié)合的機(jī)械手視覺伺服控制方法有效性與優(yōu)越性檢驗(yàn)

以六自由度機(jī)械手PUMA560 與攝像頭構(gòu)成的單目眼在手(eye-in-hand型結(jié)構(gòu))視覺伺服系統(tǒng)為對象，利用MATLAB進(jìn)行仿真實(shí)驗(yàn)。實(shí)驗(yàn)中相機(jī)焦距f取0．008，像素為1024×1024，主點(diǎn)為(512，512)，像素大小(10-5，10-5)。取目標(biāo)像素坐標(biāo)為[u1u2u3u4；v1v2v3v4]=[412 412 612 612；412 612 612 412]。在實(shí)驗(yàn)中，為了避免圖像誤差過大對模糊邏輯控制器的影響，在模糊邏輯中使模糊輸入||e||=||0．1e||。在使用卡爾曼在線估計(jì)時(shí)取過程噪聲協(xié)方差Q=0．5，觀測噪聲協(xié)方差R=6，狀態(tài)估計(jì)誤差協(xié)方差的初始值為P(0)=103Imn。狀態(tài)變量初始值為：

分別對使用傳統(tǒng)的固定增益的比例控制器(比例系數(shù)λ=0．5，相機(jī)深度Z=3m)的控制系統(tǒng)和本文提出的控制方法進(jìn)行仿真，仿真結(jié)果如圖4和圖5。

圖4 傳統(tǒng)的IBVS誤差隨時(shí)間的變化

圖5 本文控制方法下圖像誤差隨時(shí)間的變化

對比圖4與圖5誤差曲線可知，傳統(tǒng)的固定增益的視覺伺服控制方法四個(gè)特征點(diǎn)像素坐標(biāo)(u1，v1)、(u2，v2)、(u3，v3)、(u4，v4)的誤差收斂所需時(shí)間為8s 左右，而使用本文提出的控制方法時(shí)圖像誤差的收斂時(shí)間僅為3s 左右，顯著提高了控制系統(tǒng)的收斂時(shí)間，表明本文提出的控制方法的有效性與優(yōu)越性，能夠使機(jī)械手在視覺伺服控制下完成精確定位任務(wù)。

6 結(jié)束語

傳統(tǒng)的視覺伺服控制方法在求取圖像雅可比矩陣時(shí)需要提前獲取相機(jī)的參數(shù)，而相機(jī)參數(shù)的獲取需要進(jìn)行復(fù)雜的相機(jī)標(biāo)定實(shí)驗(yàn)，制約了視覺伺服控制技術(shù)的推廣。本文利用卡爾曼濾波算法在線估計(jì)圖像雅可比矩陣，避免了復(fù)雜的相機(jī)標(biāo)定工作，提高了工作效率。另外，使用卡爾曼濾波算法估計(jì)圖像雅克比矩陣時(shí)引入了噪聲，可能導(dǎo)致控制系統(tǒng)收斂速度過慢。為了解決這一問題，利用模糊邏輯的思想設(shè)計(jì)了一種模糊控制器，能夠使視覺伺服控制器系數(shù)λ的值隨圖像誤差自適應(yīng)的變化，提高了控制系統(tǒng)的收斂速度。最后，通過仿真實(shí)驗(yàn)驗(yàn)證了所提出的模糊邏輯與卡爾曼濾波相結(jié)合的視覺伺服控制方法的有效性，與傳統(tǒng)控制方法的對比結(jié)果表明所提出控制方法的優(yōu)越性。