【正文】 ，所以不但可以對(duì)平穩(wěn)的一維隨機(jī)過(guò)程進(jìn)行估計(jì)，也可以對(duì)非平穩(wěn)的﹑多維隨機(jī)過(guò)程進(jìn)行估計(jì)。Kalman濾波實(shí)際上是一種最優(yōu)估計(jì)方法【16】。設(shè)隨機(jī)線性離散系統(tǒng)采用如下?tīng)顟B(tài)方程表示為： (31) (32)式中，為系統(tǒng)的狀態(tài)向量，是系統(tǒng)的觀測(cè)序列，是維系統(tǒng)過(guò)程噪聲序列，是維觀測(cè)噪聲序列，矩陣是系統(tǒng)的維狀態(tài)轉(zhuǎn)移矩陣，是維噪聲輸入矩陣。關(guān)于系統(tǒng)過(guò)程噪聲和觀測(cè)噪聲的統(tǒng)計(jì)特性，可假設(shè)如下： 其中，是系統(tǒng)過(guò)程噪聲的維對(duì)稱(chēng)非負(fù)定方差矩陣，是系統(tǒng)觀測(cè)噪聲的維對(duì)稱(chēng)正定方差陣。 利用這些信息，我們可以建立Kalman濾波估計(jì)的遞推形式，對(duì)系統(tǒng)狀態(tài)進(jìn)行估計(jì)，下面直接給出隨機(jī)線性離散系統(tǒng)的Kalman濾波方程。狀態(tài)一步預(yù)測(cè)： (33) 協(xié)方差預(yù)測(cè)： (34) 狀態(tài)估計(jì)： (35) 濾波增益矩陣： (36) 估計(jì)誤差方差陣： (37)上面給出的遞推的Kalman濾波算法是通用的，也是相當(dāng)有效的。而且這種方法經(jīng)過(guò)修正后，還可用來(lái)進(jìn)行狀態(tài)空間參數(shù)模型參數(shù)的估計(jì)。在一個(gè)濾波周期內(nèi)，從Kalman濾波在使用系統(tǒng)信息和觀測(cè)信息的先后次序來(lái)看，Kalman濾波具有兩個(gè)明顯的信息更新過(guò)程：時(shí)間更新過(guò)程和觀測(cè)更新過(guò)程。如下圖所示：圖31 kalman濾波原理圖【17] 設(shè)對(duì)象為二階傳遞函數(shù)：取采樣時(shí)間為1ms，采用Z變換將對(duì)象離散化，并描述為離散狀態(tài)方程的形式：帶有測(cè)量噪聲的被控對(duì)象輸出為：式中，，，、。采用卡爾曼濾波器實(shí)現(xiàn)信號(hào)的濾波，取。仿真時(shí)間為3s，原始信號(hào)及帶有噪聲的原始信號(hào)、原始信號(hào)及濾波后的信號(hào)和誤差協(xié)方差的變化分別如圖所示： 圖32 原始信號(hào)及帶有噪聲的原始信號(hào)圖33 原始信號(hào)及濾波后的信號(hào)圖34 誤差協(xié)方差的變化仿真結(jié)果表明，該濾波器對(duì)控制干擾和測(cè)量噪聲具有很好的濾波作用。 因?yàn)楣潭ㄑ鄣膱D像雅可比矩陣在機(jī)器人運(yùn)動(dòng)過(guò)程中是不斷變化的。在應(yīng)用此矩陣建立圖像反饋控制器時(shí)，為保證控制性能，有必要對(duì)其進(jìn)行實(shí)時(shí)的辨識(shí)。這里可以使用Kalman濾波算法實(shí)現(xiàn)全局視覺(jué)雅可比矩陣的在線估計(jì)。定義觀察參數(shù)向量x為一個(gè)4維向量，包含雅可比矩陣的所有元素。 (38)式中，，分別為雅可比矩陣的四個(gè)組成元素。由雅可比矩陣的定義,有 (39)寫(xiě)成如下?tīng)顟B(tài)方程：其中，分別為狀態(tài)噪聲和圖像觀察噪聲，假定為高斯白噪聲。建立Kalman濾波遞推估計(jì)： (310) (311) (312) (313) (314) 其中，為噪聲方差陣。取為, 。其初始值可取 (意為維單位陣)。采用如下如下方法獲得：在初始位置任意給定兩步線性無(wú)關(guān)的試探運(yùn)動(dòng)，而在固定攝像機(jī)中觀察到手爪相應(yīng)的圖像特征變化為，從而獲得初始矩陣的估計(jì)值為： (315) 然后用構(gòu)成。目前存在的基于圖像的機(jī)器人視覺(jué)定位方法大部分是建立在圖像雅可比矩陣的基礎(chǔ)上的，其結(jié)構(gòu)如圖35所示。其中內(nèi)環(huán)為關(guān)節(jié)伺服控制，通過(guò)關(guān)節(jié)位置反饋來(lái)實(shí)現(xiàn)機(jī)器人軌跡跟蹤控制。在高性能伺服控制器作用下可將機(jī)器人控制系統(tǒng)近似為線性環(huán)節(jié)。外環(huán)為視覺(jué)控制器，以較低的采樣速率完成關(guān)節(jié)角設(shè)定。雙環(huán)結(jié)構(gòu)將機(jī)器人非線性運(yùn)動(dòng)控制從視覺(jué)控制系統(tǒng)中分離出來(lái)，在視覺(jué)控制中可作為線性環(huán)節(jié)處理，簡(jiǎn)化了視覺(jué)控制器的設(shè)計(jì)過(guò)程。由于現(xiàn)有機(jī)器人大多都有接收笛卡爾速度給定或位置增量指令的接口，因此雙環(huán)結(jié)構(gòu)簡(jiǎn)單易行，被廣泛采用。視覺(jué)控制器坐標(biāo)變換與軌跡規(guī)劃?rùn)C(jī)器人控制器機(jī)器人目標(biāo)點(diǎn)圖像采集圖像處理圖35 基于圖像的機(jī)器人視覺(jué)伺服系統(tǒng)在MATLAB平臺(tái)上對(duì)上文所述的基于kalman濾波的機(jī)器人（眼固定）系統(tǒng)的固定目標(biāo)定位過(guò)程進(jìn)行仿真。仿真模型如下圖所示：圖36 基于kalman濾波的機(jī)器人眼固定模型在固定目標(biāo)定位實(shí)驗(yàn)中，Kalman濾波估計(jì)算法中的、分別取為，P矩陣的初始值可取 (意為維單位陣)。圖像雅可比矩陣的初值通過(guò)一開(kāi)始作任意兩步試探運(yùn)動(dòng)得到，將其構(gòu)成：手爪初始位置=[ 0],=[ ],=[ ]，所以==[ ]，==[ ]。手爪在圖像平面坐標(biāo)=[ ]，=[ ]，=[ ]，==[ ] ==[ ],則初始矩陣的估計(jì)值為：所以在該定位實(shí)驗(yàn)中。手爪在世界坐標(biāo)上的起始位置為：[ 0 0]，即圖像坐標(biāo)為[ ]；期望的目標(biāo)為：[ ]，即圖像坐標(biāo)為[ ]。實(shí)驗(yàn)最后的得到的誤差為E=（pixel）。仿真結(jié)果如下所示：圖37 手爪在圖像平面上的運(yùn)動(dòng)軌跡(kalman濾波)圖38 圖像平面定位誤差曲線(kalman濾波)圖39 x，y方向定位誤差曲線(kalman濾波)實(shí)驗(yàn)結(jié)果分析：從上圖可以看出，機(jī)器人末端執(zhí)行器能快速跟上目標(biāo)，且誤差很小。4機(jī)器人無(wú)標(biāo)定視覺(jué)伺服實(shí)驗(yàn)系統(tǒng)的硬件部分主要由: MOTOMANSV3機(jī)器人本體、彩色CCD攝像頭、機(jī)器人控制柜、控制計(jì)算機(jī)、圖像采集卡等組成。CCD攝像頭可以固定安裝在機(jī)器人運(yùn)動(dòng)空間中一個(gè)任意位置，也可以固定安裝于機(jī)器人的末端執(zhí)行器上。圖41所示的系統(tǒng)中CCD攝像頭固定在機(jī)器人斜上方，可同時(shí)觀察目標(biāo)和手爪運(yùn)動(dòng)。經(jīng)過(guò)目標(biāo)圖像和手爪圖像的獲取,圖像處理和識(shí)別得到目標(biāo)和手爪在圖像中的位置。整個(gè)視覺(jué)伺服系統(tǒng)可以分為機(jī)器人控制子系統(tǒng)和視覺(jué)信息處理子系統(tǒng)兩部分組成。圖41 機(jī)器人無(wú)標(biāo)定視覺(jué)伺服實(shí)驗(yàn)系統(tǒng)示意圖 機(jī)器人控制子系統(tǒng)機(jī)器人控制子系統(tǒng)主要完成視覺(jué)控制量的計(jì)算和機(jī)器人運(yùn)動(dòng)控制。其中視覺(jué)控制量是根據(jù)視覺(jué)信息處理子系統(tǒng)得到的圖象反饋信息計(jì)算得到的。實(shí)驗(yàn)中所用的機(jī)器人是日本安川公司開(kāi)發(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所示。圖42 MOTOMAN機(jī)器人各關(guān)節(jié)的旋轉(zhuǎn)方向定義關(guān)節(jié)i關(guān)節(jié)變量范圍(o)11509000(170,+170)2260180090(45,+150)3609000(70,+190)40902600(180,+180)509000(135,+135)60180900(350,+350)表41 MOTOMAN機(jī)器人的有關(guān)參數(shù)通過(guò)MOTOMANCOM32軟件實(shí)現(xiàn)主機(jī)和YASKAWAG工業(yè)機(jī)器人控制器YASNAC XRC之間的數(shù)據(jù)傳輸。根據(jù)圖像反饋信息計(jì)算出來(lái)的視覺(jué)控制量即為了減小機(jī)器人手爪和目標(biāo)之間的距離機(jī)器人應(yīng)運(yùn)動(dòng)的位姿增量就是通過(guò)MOTOMANCOM32軟件所提供的機(jī)器人控制接口函數(shù)傳送給機(jī)器人控制器的。常用的機(jī)器人控制接口函數(shù)有：BscIsRobotPos()：在指定的坐標(biāo)系中讀取機(jī)器人當(dāng)前的位姿信息，其中位置信息的單位為，姿態(tài)信息單位為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)單。MOTOMANCOM32軟件提供的通信協(xié)議有兩種：RS232C或TCP/IP。本實(shí)驗(yàn)系統(tǒng)采用串口通信RS232C，通過(guò)函數(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ù)位長(zhǎng)度：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)量大體相等，所走過(guò)的路徑接近于一條光滑曲線。在這條復(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)行的。視覺(jué)信息處理子系統(tǒng)主要由CCD攝像機(jī)、圖像采集設(shè)備和計(jì)算機(jī)組成，其相關(guān)配置如下：a. CCD攝像機(jī)可供機(jī)器人視覺(jué)系統(tǒng)選用的攝像機(jī)包括攝像管和CCD。攝像管是一種已發(fā)展成熟的商品，CCD是近幾年發(fā)展起來(lái)的新技術(shù)，由于體積小、重量輕、壽命長(zhǎng)且抗沖擊等特點(diǎn)，工作起來(lái)比較可靠，因此被廣泛的應(yīng)用在機(jī)器人視覺(jué)中。本實(shí)驗(yàn)系統(tǒng)采用SONY 1/3 CCD彩色攝像頭，其CCD陣列含有像素個(gè)數(shù)為752(水平)582(垂直)，(水平) (垂直)mm(等于1/3英寸攝像管的掃描面積)，水平析像力為480行，焦距為8mm，重量為380g。 b. 圖像采集設(shè)備圖像采集設(shè)備采用北京微視電子技術(shù)有限責(zé)任公司生產(chǎn)的MVPCI系列產(chǎn)品中的MVPCIV3A 6通道圖像采集卡。這是一款專(zhuān)業(yè)圖像采集卡，可以真彩、偽彩、黑白三種方式采集圖像。其結(jié)構(gòu)如圖43所示。圖43 MVPCIV3A結(jié)構(gòu)圖由于采用PCI總線結(jié)構(gòu)，圖像數(shù)據(jù)可以實(shí)時(shí)存儲(chǔ)到計(jì)算機(jī)內(nèi)存中，并在屏幕上顯示。此外由于借用了計(jì)算機(jī)的內(nèi)存，極大的提高了處理速度。本實(shí)驗(yàn)系統(tǒng)只使用了一個(gè)攝像機(jī)，故只用到一個(gè)通道。實(shí)驗(yàn)中所用到的MVPCIV3A 圖像采集卡的主要函數(shù)有1) 實(shí)時(shí)顯示 MV_EnableVGA