【正文】 n 濾波的雅可比矩陣在線辨識(shí) 因?yàn)楣潭ㄑ鄣膱D像雅可比矩陣在機(jī)器人運(yùn)動(dòng)過程中是不斷變化的。在應(yīng)用此矩陣建立圖像反饋控制器時(shí)，為保證控制性能，有必要對(duì)其進(jìn)行實(shí)時(shí)的辨識(shí)。 這里可以使用 Kalman 濾波算法實(shí)現(xiàn)全局視覺雅可比矩陣 gJ 的在線估計(jì)。定義觀察參數(shù)向量 x 為一個(gè) 4 維向量，包含雅可比矩陣 gJ 的所有元素。 1 2 3 4[ ] [ ]g g g g Tg g g gb b b bu u v vx x x x x x y x y? ? ? ??? ? ? ? ? (38) 式中 ggbux??， ggbuy??， ggbvx??， ggbvy??分別為雅可比矩陣 gJ 的四個(gè)組成元素。 由雅可比矩陣的定義 ,有 ( 1 ) ( ) ( ) ( )g g ggf k f k J k p k? ? ? ?? (39) 白蕾：工業(yè)機(jī)器人視覺伺服 22 寫成如下狀態(tài)方程： ( 1) ( ) ( )x k x k k?? ? ? ( ) ( 1 ) ( ) ( ) ( ) ( )ggy k f k f k C k x k v k? ? ? ? ? ? 其中 ()k? ， ()vk 分別為狀態(tài)噪聲和圖像觀察噪聲，假定為高斯白噪聲。 24( ) 0() 0 ( )gTgTpkCk pk????? ????? 建立 Kalman 濾波遞推估計(jì)： ? ?( 1) ( )x k x k? ?? (310) ( 1) ( )Q k P k R?? ? ? (311) ? ? ?( 1 ) ( 1 ) ( 1 ) [ ( 1 ) ( ) ( ) ]x k x k K k y k C k x k?? ? ? ? ? ? ? ? (312) 1( 1 ) ( 1 ) ( ) [ ( ) ( 1 ) ( ) ]TT vK k Q k C k C k Q K C k R ?? ? ? ? ? (313) ( 1 ) [ ( 1 ) ( ) ] ( 1 )P k I K k C k Q k? ? ? ? ? (314) 其中 R? ， vR 為噪聲方差陣。取為 ? ? , ? 。其初始值可取 5(0) 10 mnPI? ( sI意為 s 維單位陣 )。 ?(0)x 采用如下如下方法獲得： 在初始位置任意給定兩步線性無關(guān)的試探運(yùn)動(dòng) 12,ggpp??，而在固定攝像機(jī)中觀察到手爪相應(yīng)的圖像特征變化為 12,ggff??，從而獲得初始 J 矩陣的估計(jì)值為： 11 2 1 2? ( 0 ) [ ] [ ]g g g ggJ f f p p ?? ? ? ? ? (315) 然后用 ?(0)gJ 構(gòu)成 ?(0)x 。 基于 kalman 濾波的機(jī)器人無標(biāo)定視覺 仿真 目前存在的基于圖像的機(jī)器人視覺定位方法大部分是建立在圖像雅可比矩陣的基礎(chǔ)上的，其結(jié)構(gòu)如圖 35 所示。其中內(nèi)環(huán)為關(guān)節(jié)伺服控制，通過關(guān)節(jié)位置反饋來實(shí)現(xiàn)機(jī)器人 軌跡跟蹤控制。在高性能伺服控制器作用下可將機(jī)器人控制系統(tǒng)近似為線性環(huán)節(jié)。外環(huán)為視覺控制器，以較低的采樣速率完成關(guān)節(jié)角設(shè)定。雙環(huán)結(jié)構(gòu)將機(jī)器人非線性運(yùn)動(dòng)控制從視覺控制系統(tǒng)中分離出來，在視覺控制中可作為線性環(huán)節(jié)處理，簡(jiǎn)化了視覺控制器的設(shè)計(jì)過程。由于現(xiàn)有機(jī)器人大多都有接收笛卡爾速度給定或位置增量指令的接口，因此雙環(huán)結(jié)構(gòu)簡(jiǎn)單易行，被廣泛采用。 西安理工大學(xué)本科生畢業(yè)設(shè)計(jì)（論文） 23 圖 35 基于圖像的機(jī)器人視覺伺服系統(tǒng) 在 MATLAB 平臺(tái)上對(duì)上文所述的 基于 kalman 濾波的機(jī)器人（眼固定） 系統(tǒng) 的固 定目標(biāo)定位過程進(jìn)行仿真 。 仿真模型如下圖所示： 圖 36 基于 kalman 濾波的機(jī)器人眼固定模型 在固定目標(biāo)定位實(shí)驗(yàn)中， Kalman 濾波估計(jì)算法中的 R? 、 vR ，分別取為 ， ，P矩陣的初始值可取 5(0) 10 mnPI? ( sI 意為 s 維單位陣 )。 圖像雅可比矩陣的初值 ?(0)J 通過一開始作任意兩步試探運(yùn)動(dòng)得到，將其構(gòu)成 ?(0)x ：手爪初始位置 0p =[ 0], 1p =[ ], 2p =[ ]， 所以 1gp? = 1p 0p =[ ]，2gp? = 2p 1p =[ ]。手爪在圖像平面坐標(biāo) 0f =[ ]，1f =[ ]， 2f =[ ]， 1gf? =1f 0f =[ ] 2gf? = 2f 1f =[ ],則初始 J 矩陣的估計(jì)值為： 11 2 1 2 0 . 0 0 5 8 0 . 0 5 1 5? ( 0 ) [ ] [ ] 1 . 0 0 0 4 * [ ]1 . 9 5 9 0 5 . 0 4 9 8g g g gJ f f p p e?? ? ? ? ? ? ? 視覺控制器 坐標(biāo)變換與軌跡規(guī)劃 機(jī)器人控制器 機(jī)器人 ? dX ? 目標(biāo)點(diǎn) df X 0p 圖像采集 圖像處理 白蕾：工業(yè)機(jī)器人視覺伺服 24 所以在 該定位實(shí)驗(yàn)中， T?x( 0) =1 .0e +0 04* [ 058 0. 051 5 1 .95 90 4 98]。 手爪在世界坐標(biāo)上的起始位置為： [ 0 0]，即圖像坐標(biāo)為 [ ]；期望的目標(biāo)為： [ ]，即圖像坐標(biāo)為 [ ]。實(shí)驗(yàn)最后的得到的誤差為 E=（ pixel）。 仿真結(jié)果如下所示： 215 220 225 230 235250252254256258260262264266268270基于 k a l m a n 濾波的圖像平面手爪定位曲線y方向 單位：像素x 方向 單位：像素機(jī)器人手爪運(yùn)動(dòng)軌跡目標(biāo)點(diǎn) 圖 37 手爪在圖像平面上的運(yùn)動(dòng)軌跡 (kalman 濾波 ) 0 5 10 15 20 2502468101214基于 k a l m a n 濾波的圖像平面定位誤差曲線圖像平面誤差 單位：像素 單位：采樣時(shí)刻 圖 38 圖像平面 定位 誤差曲線 (kalman 濾波 ) 西安理工大學(xué)本科生畢業(yè)設(shè)計(jì)（論文） 25 0 5 10 15 20 254202468101214x,y方向定位誤差 單位：像素 單位：采樣時(shí)刻x 方向定位誤差y 方向定位誤差 圖 39 x， y 方向定位 誤差曲線 (kalman 濾波 ) 實(shí)驗(yàn)結(jié)果分析： 從上圖可以看出 ，機(jī)器人末端執(zhí)行器能快速跟上目標(biāo) ，且誤差很小。 4 機(jī)器人無標(biāo)定視覺伺服實(shí)驗(yàn) 系統(tǒng)整體結(jié)構(gòu) 系統(tǒng)的硬件部分主要由 : MOTOMANSV3 機(jī)器人 本體 、彩色 CCD 攝像頭、機(jī)器人控制柜、控制計(jì)算機(jī)、圖像采集卡等組成 。 CCD 攝像頭可 以固定安裝在機(jī)器人運(yùn)動(dòng)空白蕾：工業(yè)機(jī)器人視覺伺服 26 間中一個(gè)任意位置，也可以固定安裝于機(jī)器人的末端執(zhí)行器上。圖 41 所示的系統(tǒng)中CCD 攝像頭固定 在 機(jī)器人斜上方，可同時(shí)觀察目標(biāo)和手爪運(yùn)動(dòng)。經(jīng)過 目標(biāo)圖像 和手爪圖像 的獲取 ,圖像處理和識(shí)別得到目標(biāo) 和手爪 在圖像中的位置。整個(gè)視覺伺服系統(tǒng)可以分為 機(jī)器人 控制子系統(tǒng)和視覺信息處理子系統(tǒng)兩部分組成。 圖 41 機(jī)器人無標(biāo)定視覺伺服實(shí)驗(yàn)系統(tǒng)示意圖 機(jī)器人控制子系統(tǒng) 機(jī)器人控制子系統(tǒng)主要完成視覺控制量的計(jì)算和機(jī)器人運(yùn)動(dòng)控制。其中視覺控制量是根據(jù)視覺信息處理子系統(tǒng)得到的圖象反饋信息計(jì)算得到的。實(shí)驗(yàn) 中所用的機(jī)器人是日本安川公司開發(fā)的工業(yè)機(jī)器人 MOTOMANSV3XL。整個(gè)機(jī)器人控制系統(tǒng)由機(jī)器人本體、供電電纜、 YANSNAC XRC 控制柜、 YANSNAC XRC 再現(xiàn)操作盒及 YANSNAC XRC 示教編程器五部分組成 [18]。 MOTOMANSV3XL 型機(jī)器人具有 6 個(gè)自由度，而且六個(gè)關(guān)節(jié)均為旋轉(zhuǎn)關(guān)節(jié)，每個(gè)關(guān)節(jié)都采用一個(gè)交流伺服電機(jī)單獨(dú)驅(qū)動(dòng)。其主要參數(shù)如表 41 所示，其中 6 個(gè)軸的定義如圖 42所示。 西安理工大學(xué)本科生畢業(yè)設(shè)計(jì)（論文） 27 圖 42 MOTOMAN 機(jī)器人各關(guān)節(jié)的旋轉(zhuǎn)方向定義 關(guān)節(jié) i ()ia mm ()iao ()id mm ()i o? 關(guān)節(jié)變量范圍(o) 1 150 90 0 0 (170,+170) 2 260 180 0 90 (45,+150) 3 60 90 0 0 (70,+190) 4 0 90 260 0 (180,+180) 5 0 90 0 0 (135,+135) 6 0 180 90 0 (350,+350) 表 41 MOTOMAN 機(jī)器人的有關(guān)參數(shù) 通過 MOTOMANCOM32 軟件實(shí)現(xiàn)主機(jī)和 YASKAWAG 工業(yè)機(jī)器人控制器 YASNAC XRC 之間的數(shù)據(jù)傳輸。根據(jù)圖像反饋信息計(jì)算出來的視覺控制量即為了減小機(jī)器人手爪和目標(biāo)之間的距離機(jī)器人應(yīng)運(yùn)動(dòng)的位姿增量就是通過 MOTOMANCOM32軟件所提供的機(jī)器人控制接口函數(shù)傳送給機(jī)器人控制器的。 常用的 機(jī)器人控制接口函數(shù)有 ： BscIsRobotPos()：在指定的坐標(biāo)系中讀取機(jī)器人當(dāng)前的位姿信息，其中位置信息( , , )xyz 的單位為 mm ，姿態(tài)信息 ( , , )x y zT T T 單位為 o。 BscImov()：在指定的坐標(biāo)系中實(shí)現(xiàn)增量式運(yùn)動(dòng)。 BscMov()：在指定的坐標(biāo)系中實(shí)現(xiàn)絕對(duì)位置運(yùn)動(dòng)。 BsPMov()：將機(jī)器人移動(dòng)到指定的脈沖位置。 利用這些函數(shù)可以控制機(jī)器人在不同坐標(biāo)系下的六個(gè)運(yùn)動(dòng)分量，實(shí)現(xiàn)非常簡(jiǎn)單。 白蕾：工業(yè)機(jī)器人視覺伺服 28 MOTOMANCOM32 軟件提供的通信協(xié)議有兩種： RS232C 或 TCP/IP。本實(shí)驗(yàn)系統(tǒng)采用串口通信 RS232C，通過函數(shù) BscSetCom()實(shí)現(xiàn) 串口通信相關(guān)參數(shù)的設(shè)置。 實(shí)驗(yàn)中相關(guān)參數(shù)的設(shè)置如下： 通信端口號(hào)： COM2 波特率： 9600（最大） 奇偶校驗(yàn)：偶校驗(yàn) 數(shù)據(jù)位長度： 8位 停止位： 1位 MOTOMAN 型機(jī)器人編程采用三種坐標(biāo)系對(duì)末端執(zhí)行器進(jìn)行位置和姿態(tài)標(biāo)定 ,分別是基坐標(biāo)系 (World Coordinates)、工具坐標(biāo)系 (Tool Coordinates)和關(guān)節(jié)坐標(biāo)系 (Joint Coordinates)。其中基坐標(biāo)系和工具坐標(biāo)系是直角坐標(biāo)系，分別與機(jī)器人本體的基座和末端執(zhí)行器固聯(lián)；而關(guān)節(jié)坐標(biāo)系則為廣義坐標(biāo)系，它以機(jī)器人各關(guān)節(jié)變量 組成的向量為廣義坐標(biāo)。與坐標(biāo)系相對(duì)應(yīng)， MOTOMAN 機(jī)器人控制器有兩種控制機(jī)器人的方法，一是關(guān)節(jié)插值法，這是運(yùn)動(dòng)最快的方法。在所有關(guān)節(jié)變量的起點(diǎn)和終點(diǎn)之間逐步插值，每步的移動(dòng)量大體相等，所走過的路徑接近于一條光滑曲線。在這條復(fù)雜的路線上，機(jī)器人末端不會(huì)以恒定速度運(yùn)動(dòng)。二是直線運(yùn)動(dòng)，機(jī)器人末端按指定的速度和折線路徑在基坐標(biāo)或工具坐標(biāo)系中以小步距插補(bǔ)的方式實(shí)現(xiàn)。為實(shí)現(xiàn)直線運(yùn)動(dòng)，運(yùn)動(dòng)時(shí)機(jī)器人各關(guān)節(jié)按照要求以不同的速度運(yùn)動(dòng)，這就需要進(jìn)行大量的坐標(biāo)變換運(yùn)算和機(jī)器人逆運(yùn)動(dòng)學(xué)求解，因此，直線運(yùn)動(dòng)比關(guān)節(jié)插值運(yùn)動(dòng)慢得多。但在 直角坐標(biāo)系中規(guī)劃作業(yè)路徑、運(yùn)動(dòng)方向和速度顯然要比在關(guān)節(jié)坐標(biāo)系中直觀得多，本文的實(shí)驗(yàn)都是在直角坐標(biāo)系下進(jìn)行的。 視覺信息處理子系統(tǒng) 視覺信息處理子系統(tǒng) 主要由 CCD 攝像機(jī)、圖像采集設(shè)備和計(jì)算機(jī)組成，其相關(guān)配置如下： a. CCD 攝像機(jī) 可供機(jī)器人視覺系統(tǒng)選用的攝像機(jī)包括攝像管和 CCD。攝像管是一種已發(fā)展成熟的商品， CCD 是近幾年發(fā)展起來的新技術(shù)，由于體積小、重量輕、壽命長且抗沖擊等特點(diǎn)，工作起來比較可靠，因此被廣泛的應(yīng)用在機(jī)器人視覺中。 本實(shí)驗(yàn)系統(tǒng)采用 SONY 1/3 CCD 彩色攝像頭，其 CCD 陣列含有像素個(gè)數(shù) 為 752(水平 ) ? 582(垂直 )，掃描面積為 (水平 ) ? (垂直 )mm(等于 1/3 英寸攝像管的掃描面西安理工大學(xué)本科生畢業(yè)設(shè)計(jì)（論文） 29 積 )，水平析像力為 480 行，焦距為 8mm，重量為 380g。 b. 圖像采集設(shè)備 圖像采集設(shè)備采用北京微視電子技術(shù)有限責(zé)任公司生產(chǎn)的 MVPCI 系列產(chǎn)品中的MVPCIV3A 6 通道圖像采集卡。這是一款專業(yè)圖像采集卡，可以真彩、偽彩、黑白三種方式采集圖像。其結(jié)構(gòu)如圖 43 所示。 圖 43 MVPCIV3A 結(jié)構(gòu) 圖 由于采用 PCI 總線結(jié)構(gòu)，圖像數(shù)據(jù)可以實(shí)時(shí)存儲(chǔ)到計(jì)算機(jī)內(nèi)存中，并在屏幕上顯示。此外由于借用了計(jì)算