一種基于迭代變時(shí)長(zhǎng)視覺伺服控制方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明設(shè)及工業(yè)機(jī)械臂領(lǐng)域的視覺跟蹤方法，特別是設(shè)及一種基于迭代變時(shí)長(zhǎng)策 略的視覺伺服控制方法。
【背景技術(shù)】
[0002] 機(jī)械臂視覺跟蹤是W視覺系統(tǒng)作為反饋信息控制機(jī)械臂運(yùn)動(dòng)的方法，是機(jī)械臂研 究的重要部分。視覺跟蹤是目前工業(yè)機(jī)器人研究中的熱點(diǎn)和難點(diǎn)，在工業(yè)機(jī)械臂執(zhí)行大環(huán) 境或復(fù)雜環(huán)境下任務(wù)時(shí)起到重要的作用。
[0003] 機(jī)械臂視覺跟蹤技術(shù)可W分為基于位置的方法和基于圖像的方法。其中，基于位 置的方法將視覺系統(tǒng)作為位置傳感器使用，根據(jù)位置誤差控制機(jī)械臂運(yùn)動(dòng)，運(yùn)種方法控制 器設(shè)計(jì)簡(jiǎn)單，適用于低精度跟蹤，具有誤差累積效應(yīng)。基于圖像的方法使用視覺系統(tǒng)反饋的 圖像作為反饋信息，控制機(jī)械臂運(yùn)動(dòng)到拍攝到與目標(biāo)圖像一致的場(chǎng)景處，運(yùn)種方法精度高， 但是由于基于圖像的視覺控制只有一個(gè)相機(jī)，缺少深度信息，導(dǎo)致模型的不確定性，運(yùn)樣會(huì) 影響系統(tǒng)的穩(wěn)定性和收斂性，而且要求目標(biāo)圖像在整個(gè)運(yùn)動(dòng)過程中都可見。

【發(fā)明內(nèi)容】

[0004] 本發(fā)明為了克服現(xiàn)有技術(shù)的不足，提供了一種基于迭代變時(shí)長(zhǎng)策略的機(jī)械臂視覺 伺服控制方法，該方法在保證視覺跟蹤穩(wěn)定性和收斂性的同時(shí)，不要求在運(yùn)動(dòng)過程中目標(biāo) 持續(xù)可見，實(shí)現(xiàn)成本較低。 陽〇化]本發(fā)明的目的是通過W下技術(shù)方案來實(shí)現(xiàn)的：一種基于迭代變時(shí)長(zhǎng)策略的機(jī)械臂 視覺伺服控制方法，用于機(jī)械臂目標(biāo)圖像的跟蹤，所述的機(jī)械臂為六自由度無冗余工業(yè)機(jī) 械臂，自帶單目攝像頭可實(shí)時(shí)采集圖像信息。該方法將跟蹤任務(wù)轉(zhuǎn)化到基于圖像單應(yīng)性定 義的圖像特征空間中，設(shè)計(jì)控制器在圖像特征空間中進(jìn)行軌跡跟蹤控制，包括W下步驟：
[0006] (1)手持相機(jī)，沿跟蹤軌跡拍攝目標(biāo)物體的一系列圖片作為參考圖片，所述目標(biāo)物 體的一個(gè)平面上標(biāo)注=個(gè)W上標(biāo)記點(diǎn)，并且所有參考圖片中均包含該平面；
[0007] 似機(jī)械臂開始運(yùn)動(dòng)，由攝像頭獲取圖像信息，根據(jù)其與步驟（1)得到的參考圖片 的差異，通過利用單應(yīng)性關(guān)系定義圖像特征，用六個(gè)和單應(yīng)性矩陣相關(guān)的變量作為系統(tǒng)誤 差；
[000引做將步驟似得到的系統(tǒng)誤差作為控制器輸入，由控制器輸出信號(hào)控制機(jī)械臂 運(yùn)動(dòng)，從而跟蹤步驟1得到的一系列參考圖片；所述控制器包括兩部分：反饋控制器和迭代 控制器，所述反饋控制器為P反饋控制器，所述迭代控制器為PID學(xué)習(xí)率迭代控制器；
[0009] (4)在機(jī)械臂運(yùn)動(dòng)過程中出現(xiàn)目標(biāo)物體超出視野范圍時(shí)，此次迭代結(jié)束，記迭代時(shí) 間為Tk,Tk時(shí)間后，控制器的輸入值、系統(tǒng)的誤差和誤差變化率均為零；
[0010] (5)迭代多次控制機(jī)械臂運(yùn)動(dòng)，直到拍攝圖片和參考圖片誤差滿足終止條件或者 達(dá)到迭代次數(shù)，實(shí)現(xiàn)按照參考軌跡的機(jī)械臂視覺伺服跟蹤控制。 W11] 步驟似中所述的單應(yīng)性圖像特征由公式（4)定義，公式（4)為：
[0013] 其中，H為單應(yīng)性矩陣，（R，t)表示了機(jī)械臂的運(yùn)動(dòng)狀態(tài)，n為相機(jī)坐標(biāo)系下地面的 法向量，d為相機(jī)坐標(biāo)系原點(diǎn)與地面之間的距離，K為相機(jī)內(nèi)部參數(shù)矩陣。
[0014] 步驟（2)中所述利用單應(yīng)性關(guān)系定義圖像特征具體為：依次求得廣義單應(yīng)性矩陣 和狹義單應(yīng)性矩陣；
[0015] 所述廣義單應(yīng)性矩陣G如公式（1)所示：
[0017] 其中，m和Hif分別表示同一特征在當(dāng)前圖片和參考圖片的像素點(diǎn)位置坐標(biāo)，Z和Zf分別表示同一特征點(diǎn)在當(dāng)前圖像坐標(biāo)系和參考圖像坐標(biāo)系下的深度；
[001引所述狹義單應(yīng)性矩陣H如公式似所示：
[0019]H=RiGK似
[0021] K為相機(jī)內(nèi)部參數(shù)矩陣，f。、f；分別表示焦距在像素坐標(biāo)軸上對(duì)應(yīng)的像素點(diǎn)數(shù)目。 陽0巧步驟似中所述系統(tǒng)誤差e如公式（5) (6) (7)所示，如下：
[002引e= [e*ejT妨
陽0 巧]Gf=Vex(H-HT) (7)
[0026] 步驟（3)中，所述P反饋控制器具體為：控制器輸出皆貨和本次迭代上個(gè)時(shí)刻的 系統(tǒng)誤差e(t)成正比例關(guān)系，如公式（8)所示： 陽0八]= (8)
[00測(cè)其中，Kfd為反饋控制器的比例系數(shù)；
[0029] 所述PID學(xué)習(xí)率迭代控制器具體為：控制器輸出獄與上次迭代的輸出Uk1 (t)、 系統(tǒng)誤差6ki(t)和系統(tǒng)誤差變化率關(guān)系如公式（9)所示：
[0030] （n= ",_,(?) +KpA-,…+Kd為-1W(9)
[0031] 其中，Kp為迭代控制器的P控制系數(shù)，Kd為迭代控制器的D控制系數(shù)；
[0032] 所述控制器輸出Uk(t)如公式（10)所示：
[0033] 。'任(/) +H'e(。（ 10 )
[0034] 步驟（4)迭代變時(shí)長(zhǎng)策略如公式（11)所示，如下： <11)
[0036] 其中，Tk為第k次的迭代時(shí)間，Td為跟蹤軌跡的期望時(shí)間。
[0037] 本發(fā)明的有益效果是，本發(fā)明基于迭代變時(shí)長(zhǎng)策略的改進(jìn)圖像跟蹤視覺方法，在 傳統(tǒng)的基于圖像視覺伺服控制方式的基礎(chǔ)上予W改進(jìn)，采用迭代的方式，克服由于深度不 確定導(dǎo)致的系統(tǒng)不穩(wěn)定性，而且又針對(duì)基于圖像要求的目標(biāo)持續(xù)可見性提出新的方法，通 過變時(shí)長(zhǎng)的方式在一定程度上克服了運(yùn)樣的視野約束。
【附圖說明】 陽03引圖1是本發(fā)明的流程圖；
[0039] 圖2是機(jī)械臂物理裝置圖；
[0040] 圖3是控制過程中物體在圖像中的位姿圖；
[0041] 圖4是某次迭代終止圖；
[0042] 圖5單應(yīng)性矩陣圖。
【具體實(shí)施方式】
[0043] 下面結(jié)合附圖和具體實(shí)施例對(duì)本發(fā)明作進(jìn)一步詳細(xì)說明。
[0044] 所謂迭代變時(shí)長(zhǎng)控制策略，即機(jī)械臂多次跟蹤在圖像空間中的軌跡，每次跟蹤受 到兩方面的影響，一為本次迭代的反饋誤差，二為上次迭代的輸入和誤差，如果目標(biāo)物體跑 出視野外，則此次迭代結(jié)束。
[0045] 本發(fā)明方法用于工業(yè)機(jī)械臂跟蹤一系列目標(biāo)圖像的場(chǎng)景，首先手持相機(jī)，得到一 系列的參考圖片集合a'}，I'表示第j幅圖片。提取參考圖片的特征點(diǎn)，根據(jù)特征點(diǎn)像素位 置得到與參考圖片的單應(yīng)性矩陣，如圖5所示，從而得到系統(tǒng)誤差。采用迭代和反饋結(jié)合的 控制方案，即控制器輸出和本次迭代上個(gè)時(shí)刻誤差有關(guān)，同時(shí)也和之前的迭代信息相關(guān)，整 個(gè)控制框圖如圖1所示。如果某次迭代過程中目標(biāo)物體跑出視野外，如圖4所示，則此次迭 代終止，下次迭代可W利用上次迭代終止前的信息，運(yùn)樣可W有效利用每次迭代的信息。具 體包括W下步驟：
[0046] 步驟1:手持相機(jī)，沿跟蹤軌跡拍攝目標(biāo)物體的一系列圖片，得到參考圖片集合 忙'}，每表示第j幅圖片，最后一張圖片為目標(biāo)位姿，女曰圖3所示；所述目標(biāo)物體的一個(gè)平 面上標(biāo)注9個(gè)標(biāo)記點(diǎn)，并且所有參考圖片中均包含該平面，如圖1所示；
[0047] 步驟2 :機(jī)械臂運(yùn)行過程中，實(shí)時(shí)拍攝圖片（I'}，利用Sift算法提取特征點(diǎn)，根據(jù) 兩個(gè)圖片I'和據(jù)特征點(diǎn)像素點(diǎn)位置，求得單應(yīng)性矩陣，如公式（1)所示