基于視覺反饋的機(jī)械臂位置模糊控制

解霄鵬，王佳瑋，高波，于凌濤

1. 哈爾濱工程大學(xué) 機(jī)電工程學(xué)院，黑龍江 哈爾濱 150001

2. 中國航空工業(yè)集團(tuán)公司 哈爾濱空氣動力研究所，黑龍江 哈爾濱 150009

機(jī)械臂的位置控制是機(jī)械臂控制領(lǐng)域的重要組成部分，合理的工作軌跡能夠提高效率，降低能耗，因此研究機(jī)械臂的位置控制具有重要意義[1]。傳統(tǒng)的位置控制方法需要獲取機(jī)械臂的準(zhǔn)確尺寸，建立運(yùn)動學(xué)模型，進(jìn)行笛卡爾空間規(guī)劃或關(guān)節(jié)空間規(guī)劃，計(jì)算量較大[2]。并且傳統(tǒng)方法很難用于對柔性機(jī)械臂的控制，因?yàn)槿嵝詸C(jī)械臂的振動大大提高了建模復(fù)雜程度，需要融合多種智能算法進(jìn)行深入的數(shù)學(xué)分析，很難實(shí)現(xiàn)誤差補(bǔ)償[3-4]。如今，利用視覺反饋技術(shù)的機(jī)械臂在實(shí)際生活中有了大規(guī)模的應(yīng)用[5]。視覺傳感器因具有精確性高、易用性強(qiáng)、獲取信息完備以及信號范圍廣等特點(diǎn)，被用于許多場景[6]。本文將針對傳統(tǒng)機(jī)械臂位置控制方法的不足，結(jié)合視覺反饋技術(shù)，提出一種機(jī)械臂位置的模糊控制方法，以解決傳統(tǒng)方法受尺寸約束的問題，并初步用于柔性機(jī)械臂的研究。

1 總體方案設(shè)計(jì)

本文研究的機(jī)械臂具有3 個自由度，均為轉(zhuǎn)動關(guān)節(jié)。第1 個轉(zhuǎn)動關(guān)節(jié)在xoy平面內(nèi)運(yùn)動；第2 個轉(zhuǎn)動關(guān)節(jié)和第3 個轉(zhuǎn)動關(guān)節(jié)共面，在xoz平面內(nèi)運(yùn)動。如圖1 所示，視覺傳感器1 布置在機(jī)械臂上方，捕捉xoy平面信息，視覺傳感器2 布置在機(jī)械臂前方，捕捉xoz平面信息，包括各個轉(zhuǎn)動關(guān)節(jié)的坐標(biāo)、機(jī)械臂末端的坐標(biāo)、目標(biāo)物的坐標(biāo)。

圖1 機(jī)械臂示意

在得到上述信息后，采用設(shè)計(jì)好的運(yùn)動方向判斷算法，即可得到各個轉(zhuǎn)動關(guān)節(jié)的運(yùn)動方向，讓機(jī)械臂末端逐步靠近目標(biāo)物體。在機(jī)械臂運(yùn)動過程中，各個關(guān)節(jié)的運(yùn)動速度通過模糊控制方法搭建二維模糊控制器進(jìn)行求解。將上述2 種方法結(jié)合即可實(shí)現(xiàn)對機(jī)械臂的位置控制，完成對目標(biāo)物的抓取。設(shè)計(jì)好核心算法后，在機(jī)器人操作系統(tǒng)（robot operating system，ROS）中建立機(jī)械臂模型，聯(lián)合MATLAB 進(jìn)行仿真實(shí)驗(yàn)驗(yàn)證，系統(tǒng)整體架構(gòu)如圖2 所示。

圖2 系統(tǒng)整體架構(gòu)

2 運(yùn)動方向判斷算法設(shè)計(jì)

在進(jìn)行機(jī)械臂運(yùn)動方向判斷時，要調(diào)用視覺傳感器得到機(jī)械臂的3 個關(guān)節(jié)、機(jī)械臂末端及目標(biāo)物體分別在xoy和xoz平面的坐標(biāo)，如表1 所示。

表1 通過視覺傳感器得到的坐標(biāo)

2.1 xoy 平面運(yùn)動方向判斷

在xoy平面，如果機(jī)械臂末端與目標(biāo)物不重合，可以通過部分關(guān)鍵點(diǎn)坐標(biāo)對關(guān)節(jié)1 和關(guān)節(jié)2 的運(yùn)動方向進(jìn)行判斷。首先判斷該平面機(jī)械臂末端到目標(biāo)物的距離是否為0，如果dm45為0 則關(guān)節(jié)1 不轉(zhuǎn)動，如果dm45不為0 則依據(jù)S1的正負(fù)判斷關(guān)節(jié)2 的轉(zhuǎn)向：S1＞0時關(guān)節(jié)2 逆時針轉(zhuǎn)動，S1＜0時關(guān)節(jié)2 順時針轉(zhuǎn)動。

判斷流程如圖3 所示，規(guī)定逆時針方向?yàn)殛P(guān)節(jié)轉(zhuǎn)動的正方向，ri值代表3 個關(guān)節(jié)的旋轉(zhuǎn)方向，ri=0代表關(guān)節(jié)不轉(zhuǎn)動，ri=1代表關(guān)節(jié)逆時針轉(zhuǎn)動，ri=-1代表關(guān)節(jié)順時針轉(zhuǎn)動。

圖3 xoy 平面運(yùn)動方向判斷流程

2.2 xoz 平面運(yùn)動方向判斷

2.2.1 關(guān)節(jié)1 和關(guān)節(jié)2 運(yùn)動方向判斷在2.1 節(jié)中已通過關(guān)鍵點(diǎn)坐標(biāo)對關(guān)節(jié)1 和關(guān)節(jié)2 的運(yùn)動方向做出初步判斷。如果在xoz平面上機(jī)械臂末端與目標(biāo)物不重合，其運(yùn)動方向仍需進(jìn)一步分析，并且對關(guān)節(jié)3 的運(yùn)動方向進(jìn)行判斷。如圖4 所示，在xoz平面上，判斷該平面機(jī)械臂末端到目標(biāo)物的距離是否為0，如果為0 則關(guān)節(jié)2 和關(guān)節(jié)3 不轉(zhuǎn)動，如果不為0 則通過S2和S3判斷關(guān)節(jié)1 和關(guān)節(jié)2 的轉(zhuǎn)向。

當(dāng)S2＞0時關(guān)節(jié)1 逆時針轉(zhuǎn)動，當(dāng)S2＜0時關(guān)節(jié)1順時針轉(zhuǎn)動；當(dāng)S3＞0時關(guān)節(jié)2 逆時針轉(zhuǎn)動，當(dāng)S3＜0時關(guān)節(jié)2 順時針轉(zhuǎn)動。判斷流程如圖5 所示。

圖5 關(guān)節(jié)1、2 在xoz 平面運(yùn)動方向判斷流程

2.2.2 關(guān)節(jié)3 運(yùn)動方向判斷

對于關(guān)節(jié)3 轉(zhuǎn)動方向的判斷，首先要用獲得的坐標(biāo)求出關(guān)節(jié)2 和機(jī)械臂末端之間的距離dn24、關(guān)節(jié)2 和目標(biāo)物之間的距離dn25，再根據(jù)dn24和dn25的大小進(jìn)行判斷。如果兩者相等，則關(guān)節(jié)3 不轉(zhuǎn)動；如果兩者大小不等，則進(jìn)一步根據(jù)目標(biāo)物和關(guān)節(jié)2 在xoz平面上的坐標(biāo)yn2和yn5的大小進(jìn)行判斷。如果yn5≥yn2且dn24＞dn25，關(guān)節(jié)3 順時針轉(zhuǎn)動，反之關(guān)節(jié)3 逆時針轉(zhuǎn)動；如果yn5＜yn2且dn24＞dn25，關(guān)節(jié)3 逆時針轉(zhuǎn)動，反之關(guān)節(jié)3 順指針轉(zhuǎn)動。關(guān)節(jié)3 判斷流程如圖6 所示。

圖6 關(guān)節(jié)3 在xoz 平面運(yùn)動方向判斷流程

3 模糊控制系統(tǒng)設(shè)計(jì)

模糊控制以模糊數(shù)學(xué)為基本理論，主要利用模糊推理技術(shù)進(jìn)行控制[7]，是一種高效的非線性控制[8]。它可用于模型參數(shù)不確定或者波動較大的線性和非線性系統(tǒng)的控制[9-10]。本文采用模糊控制方法對機(jī)械臂速度進(jìn)行控制。當(dāng)末端距離目標(biāo)物較遠(yuǎn)時，控制各關(guān)節(jié)以較大的速度運(yùn)動；隨著末端逼近目標(biāo)物，速度逐漸減小。根據(jù)以上思路利用模糊控制工具箱，設(shè)定參數(shù)，完成模糊控制系統(tǒng)的設(shè)計(jì)。

3.1 輸入量和輸出量確定

常見的模糊控制器有一維模糊控制器和二維模糊控制器[11]。一維模糊控制器以偏差e作為輸入量，二維模糊控制器以偏差e、偏差變化率ec作為輸入量。二維模糊控制器與一維模糊控制器相比，雖然結(jié)構(gòu)較為復(fù)雜，但是動態(tài)性能更好[12]，因此本文選用二維模糊控制器。

將機(jī)械臂的末端與目標(biāo)物中心距離d以及其偏差變化率dc作為輸入量；電機(jī)轉(zhuǎn)速n為輸出量，輸出量個數(shù)為3，對應(yīng)各關(guān)節(jié)的轉(zhuǎn)速。輸入量的計(jì)算需要獲取機(jī)械臂的末端坐標(biāo)Pi(xi,yi)和目標(biāo)物坐標(biāo)P(x,y)，di為距離偏差，dci為偏差變化率，采樣時間間隔為 Δt，則：

3.2 模糊化

在確定輸入量和輸出量后，首先完成隸屬度函數(shù)的確定。綜合考慮整個系統(tǒng)的精度和運(yùn)算效率，將三角形隸屬度函數(shù)和高斯型隸屬度函數(shù)相結(jié)合[13]，接著確定各個變量的模糊論域空間。把d、dc、n的基本論域設(shè)置為[0,xd]、[-xdc,xdc]、 [0,yn]，對模糊空間進(jìn)行分割[14]。隨著分割級數(shù)的增多，系統(tǒng)的精度也會提高，但是會使系統(tǒng)結(jié)構(gòu)更加復(fù)雜。采用試湊的方法，先將分割級數(shù)設(shè)置為7 級，后續(xù)可根據(jù)精度要求進(jìn)行調(diào)整。D、DC、N與d、dc、n對應(yīng)，是輸入量和輸出量的模糊語言變量。D的模糊子集為{EJ, BJ, LJ, ZO, LY, BY, EY}，論域?yàn)閧0,1,2,3,4,5,6}；DC的模糊子集為{NB, NM,NS, ZO, PS, PM, PB}，論域?yàn)閧-3,-2,-1,0,1,2,3}；N的模糊子集為{ES,FS,BS,ZH,BB,FB,EB}，論域?yàn)閧0,0.5,1,1.5,2,2.5,3}。設(shè)置量化因子kd=6/xd、比例因子kn=yn/3，為了保證系統(tǒng)精度，量化因子取值范圍設(shè)置如下：

式中δ為允許的控制誤差。

比例因子kn影響系統(tǒng)的快速性和穩(wěn)定性，kn越大，系統(tǒng)的快速性越好，但是如果kn過大會出現(xiàn)超調(diào)，穩(wěn)定性下降，因此在模糊控制器結(jié)構(gòu)上加入?yún)?shù)調(diào)整器調(diào)整kn的大小。參數(shù)調(diào)整器的輸入量為d，Kn與kn對應(yīng)，Kn的模糊子集為{ES, FS, BS,ZH, BB, FB, EB}，論域?yàn)閧0, 0.5, 1, 1.5, 2, 2.5, 3}，最終確立的模糊控制系統(tǒng)結(jié)構(gòu)如圖7 所示。

圖7 模糊控制系統(tǒng)結(jié)構(gòu)

3.3 模糊規(guī)則確定

首先確定基本模糊控制器的模糊規(guī)則?；灸：刂破鞯妮斎肓繛閐和dc，文中將分割級數(shù)設(shè)置為7 級，則模糊規(guī)則庫包含49 個形式為“ifthen”的模糊條件語句，它們決定了模糊關(guān)系Ri=(A×B)T×C，由49 個模糊規(guī)則經(jīng)過運(yùn)算后可得49 個模糊關(guān)系矩陣，對它們求并可得到最終的模糊關(guān)系矩陣R。輸入量的模糊集合為D和DC，模糊推理過程為U=(D×DC)T·R，即可得到對應(yīng)控制量的模糊集合?？刂戚敵隽康哪：?guī)則如表2 所示，模糊規(guī)則為“ifDis EJ andDCis NB thenNis ES”。

表2 基本模糊控制器模糊規(guī)則表

接著確定參數(shù)調(diào)整器的模糊規(guī)則。參數(shù)調(diào)整器輸入量只有d，因此有7 個形式為“if-then”的模糊條件語句。在初始階段為了加快系統(tǒng)響應(yīng)速度，將比例因子kn設(shè)置成一個較大的值，以此來提高控制能力。當(dāng)d較大時增大kn，讓系統(tǒng)的輸出趨向穩(wěn)態(tài)；當(dāng)d減小時，也要逐步減小kn的值，使系統(tǒng)具有較好的穩(wěn)定性。控制輸出量的模糊規(guī)則如表3 所示。

表3 參數(shù)調(diào)整器模糊規(guī)則表

3.4 解模糊化

完成上述運(yùn)算后，輸出量以模糊語言的形式表示，通過解模糊化可以將模糊輸出值映射到真實(shí)世界。采用面積重心法對基本模糊控制器和參數(shù)調(diào)整器進(jìn)行解模糊化操作，它的輸出值是隸屬度函數(shù)圖與坐標(biāo)軸構(gòu)成的圖像的重心[15]。這種方法的特點(diǎn)是在進(jìn)行輸出過程中控制值更加平滑，即使輸入信號存在很小的變化，輸出信號也能夠跟著變化。面積重心法表達(dá)式為

式中：V為輸出論域，v為輸出論域中的值，μv(v)為隸屬度函數(shù)，v0為模糊推理輸出。

離散域的表達(dá)式為

式中：vi為離散域下輸出論域中的值，μv(vi)為隸屬度函數(shù)，v0為模糊推理輸出，n為輸出量化級數(shù)。

對輸出量進(jìn)行解模糊化即完成了整個模糊控制系統(tǒng)的設(shè)計(jì)。搭建完模糊控制系統(tǒng)后，通過對控制結(jié)果的觀察、調(diào)整模糊規(guī)則和隸屬度函數(shù)等，對系統(tǒng)性能進(jìn)行改善。

通過視覺傳感器獲得機(jī)械臂的末端和目標(biāo)物的坐標(biāo)，計(jì)算2 點(diǎn)的距離，并通過求解其偏差變化率作為模糊控制器的輸入量，調(diào)用模糊控制器，計(jì)算系統(tǒng)的輸出，得到其對應(yīng)輸出量，就可以實(shí)現(xiàn)對各個關(guān)節(jié)運(yùn)動速度的控制。再結(jié)合文中的運(yùn)動方向判斷算法，即可完成對機(jī)械臂的位置控制。

4 機(jī)械臂位置控制仿真分析

4.1 仿真系統(tǒng)建立

仿真系統(tǒng)整體框架如圖8 所示，在Gazebo 中搭建機(jī)械臂的三維模型，建立三維仿真環(huán)境，用以檢驗(yàn)機(jī)械臂的控制算法效果。ROS 采用話題、服務(wù)、Action 作為基本的通信手段，采集節(jié)點(diǎn)信息傳輸給MATLAB。MATLAB 會對視覺傳感器的圖像進(jìn)行處理，進(jìn)行目標(biāo)識別，獲取關(guān)鍵點(diǎn)坐標(biāo)。在獲取了3 個關(guān)節(jié)、末端、目標(biāo)物體在空間內(nèi)的坐標(biāo)后，利用運(yùn)動方向判斷算法判斷機(jī)械臂的運(yùn)動方向，通過模糊控制工具箱，將各個關(guān)節(jié)的速度信息反饋給ROS，控制機(jī)械臂三維模型進(jìn)行運(yùn)動，實(shí)現(xiàn)對機(jī)械臂的位置控制。

圖8 仿真系統(tǒng)

4.2 視覺圖像處理

為使仿真實(shí)驗(yàn)更貼合實(shí)際情況，在仿真環(huán)境中添加視覺傳感器，在各個關(guān)節(jié)及機(jī)械臂末端設(shè)置圓形標(biāo)記，通過視覺檢測算法對標(biāo)記物進(jìn)行識別，并獲取其中心坐標(biāo)。獲取圖像信息后，首先進(jìn)行圖像預(yù)處理，將其灰度化減少運(yùn)算量；再進(jìn)行二值化操作，保留其形態(tài)信息的增強(qiáng)對比度，便于后續(xù)對標(biāo)記物的識別檢測[16]。由于成像變形，圓形標(biāo)記物會變成橢圓，需要用橢圓檢測算法進(jìn)行識別?；诨舴蜃儞Q的橢圓檢測算法需要確定描述橢圓的5 個參數(shù)，不可避免地出現(xiàn)了計(jì)算量大、耗時長等問題[17-18]。本文采用基于弧線段的橢圓檢測算法。橢圓由若干弧線段組成，通過直線提?。╨ine segment detector，LSD）算法對其進(jìn)行檢測[19]。把弧線段進(jìn)行分組，通過對連續(xù)性和凸性的判定，即可得到2 個弧線段是否為同一組。分組完畢后，對跨度角進(jìn)行判斷，如跨度角近似等于360°，就將這組設(shè)置為初始橢圓集合。接著對描述橢圓的5 個參數(shù)分為橢圓中心坐標(biāo)、長軸和短軸長度、旋轉(zhuǎn)角度3 個子空間進(jìn)行聚類，減小運(yùn)算量，聚類完成后即可得到候選橢圓。為了保證檢測結(jié)果的可靠性，從覆蓋角度和邊緣點(diǎn)數(shù)量2 個角度進(jìn)行評分。覆蓋角度越大、邊緣點(diǎn)越多，說明得到的橢圓擬合效果越好，可選擇出最優(yōu)橢圓。圖像處理流程如圖9 所示。

圖9 圖像處理流程

機(jī)械臂運(yùn)動過程中，用視覺傳感器1 和視覺傳感器2 捕捉圖像信息，提供關(guān)鍵點(diǎn)坐標(biāo)，控制機(jī)械臂末端靠近目標(biāo)點(diǎn)。隨著二者之間的距離逐漸變小，會出現(xiàn)標(biāo)記物部分重疊的情況，如圖10 所示，此時無法識別橢圓。

圖10 末端和目標(biāo)物重合時橢圓檢測

為了在標(biāo)記物出現(xiàn)重合的情況下還能夠準(zhǔn)確識別，在目標(biāo)物附近安裝視覺傳感器3，并設(shè)置閾值dm。當(dāng)末端和目標(biāo)物的距離大于該閾值時，用視覺傳感器1 和視覺傳感器2 獲取圖像信息，反之調(diào)用視覺傳感器3。由于視覺傳感器3 設(shè)置在目標(biāo)物附近，不會出現(xiàn)成像變形，調(diào)用霍夫變換對圓的檢測算法即可[20]，控制末端逐漸靠近目標(biāo)物并完全重合，結(jié)果如圖11 所示。

圖11 末端靠近目標(biāo)物并完全重合

4.3 仿真結(jié)果分析

在搭建好虛擬仿真系統(tǒng)并且完成圖像處理后，開始進(jìn)行仿真實(shí)驗(yàn)。仿真結(jié)果如圖12 所示。從機(jī)械臂的末端位置變化曲線以及速度變化曲線可以看出，機(jī)械臂在初始階段以較大的速度運(yùn)行，在逐步靠近目標(biāo)物時速度逐漸降低，并且曲線較為圓滑。最終末端與目標(biāo)物之間的距離誤差為4.21×10-6m，整個過程運(yùn)動平穩(wěn)，誤差較小，說明本文建立的機(jī)械臂位置的模糊控制系統(tǒng)效果良好。

圖12 仿真結(jié)果

5 柔性機(jī)械臂探究

通過三自由度機(jī)械臂的仿真實(shí)驗(yàn)，驗(yàn)證了模糊控制方法的可靠性。由于該方法不需要建立復(fù)雜的運(yùn)動學(xué)模型，將其初步用于柔性機(jī)械臂的控制。在ROS 和MATLAB 的聯(lián)合仿真中，目前只能建立剛性模型，因此用2 根剛性機(jī)械臂來近似模擬柔性機(jī)械臂。如圖13 所示，在桿1 轉(zhuǎn)動的過程中，調(diào)用函數(shù)得到桿1 某端的速度v，將kv作為關(guān)節(jié)2 的速度（k為任選常數(shù)），桿1 和桿2 之間就會形成夾角θ。隨著桿1 的轉(zhuǎn)動，關(guān)節(jié)1 和末端的距離不斷變化，實(shí)現(xiàn)柔性機(jī)械臂的模擬。

圖13 柔性機(jī)械臂模擬

仿真實(shí)驗(yàn)中，用來模擬柔性機(jī)械臂的剛性桿具有2 個自由度，并且都位于xoy平面上。將目標(biāo)物設(shè)置在xoy平面，只需調(diào)用正視該平面的視覺傳感器1 和目標(biāo)物附近的視覺傳感器3，運(yùn)動方向的判斷參照文中三自由度機(jī)械臂2 個共面轉(zhuǎn)動關(guān)節(jié)的運(yùn)動方向判斷算法即可。其余流程與三自由度機(jī)械臂仿真實(shí)驗(yàn)一致，仿真結(jié)果如圖14 所示。機(jī)械臂在初始階段以較大速度運(yùn)行，在靠近目標(biāo)物的過程中速度逐漸降低。由于柔性機(jī)械臂長度的變化，在靠近目標(biāo)物時產(chǎn)生了輕微波動，最終末端與目標(biāo)物之間的距離誤差為2.32×10-5m，驗(yàn)證了該方法同樣適用于對柔性機(jī)械臂的控制。

圖14 柔性機(jī)械臂仿真結(jié)果

6 結(jié)論

本文提出了一種基于視覺反饋的機(jī)械臂位置的模糊控制方法，設(shè)計(jì)了運(yùn)動方向判斷算法和模糊控制器，并通過仿真實(shí)驗(yàn)驗(yàn)證了該控制系統(tǒng)的可靠性，得到結(jié)論如下：

1)該控制系統(tǒng)不受機(jī)械臂桿長的約束，適用范圍廣并且誤差較小，具有快速性與較好的穩(wěn)定性。

2)相比于傳統(tǒng)的逆運(yùn)動學(xué)解法，無需建立復(fù)雜的運(yùn)動學(xué)模型，求解過程更簡單，可用性強(qiáng)。

3)該方法已初步實(shí)現(xiàn)對柔性機(jī)械臂的控制，后續(xù)會對該方法進(jìn)行深入研究，解決柔性機(jī)械臂控制過程中存在的建模復(fù)雜等問題，具有廣泛的應(yīng)用價值。