[00創(chuàng)妒換，y。，互。)二 T廠(0|)?樂(化)W
[0043] 由前述算法坐標(biāo)系{C'l}原點(diǎn)移動至與坐標(biāo)系{T}的原點(diǎn)重合時(shí)，有
[0044] '"戶二,"尸((/) ( 10)
[0045] 所W (9)式可寫為
[0046] 另巧義，於Z)=巧牌)?;?，(伏巧磚)?聲映（化）
[0047] (11)式即為基于虛擬移動關(guān)節(jié)的多自由度雙目視覺系統(tǒng)的正運(yùn)動學(xué)方程。
[004引基于虛擬移動關(guān)節(jié)的逆運(yùn)動學(xué)算法基本思想是:將"目標(biāo)點(diǎn)在左眼相機(jī)的成像點(diǎn) 位于圖像中屯、"運(yùn)一圖像坐標(biāo)系幾何關(guān)系，轉(zhuǎn)化為"左眼相機(jī)光軸通過目標(biāo)點(diǎn)"運(yùn)一基坐標(biāo) 系幾何關(guān)系。同時(shí)將"左眼相機(jī)光軸通過目標(biāo)點(diǎn)"運(yùn)一控制目標(biāo)通過引入一個(gè)虛擬移動關(guān)節(jié) 實(shí)現(xiàn)，從而在逆運(yùn)動學(xué)求解時(shí)，通過增加1個(gè)虛擬移動關(guān)節(jié)變量d，替換掉雙目視覺算法中圖 像空間的4個(gè)參數(shù)(Ul，Vl )和(Ur，Vr )。
[0049] 據(jù)此，根據(jù)（11)式，可W將n自由度雙目視覺系統(tǒng)的逆運(yùn)動學(xué)求解轉(zhuǎn)化為n+1自由 度機(jī)械臂的逆運(yùn)動學(xué)求解，運(yùn)樣便可利用傳統(tǒng)的D-H法進(jìn)行求解。即已知目標(biāo)點(diǎn)在基坐標(biāo)系 {B}中的位置(xo，yo，zo)，求得相應(yīng)多自由度臂各關(guān)節(jié)及虛擬移動關(guān)節(jié)的位置參數(shù)：
[0050] 白 i = f(x〇,y〇,zo)，i = l，2，...，n (12)
[0051] d = f (xo,yo,zo) (13)
[0052] 其中多自由度臂各關(guān)節(jié)的A 0為真正所需求解的控制參數(shù)，虛擬移動關(guān)節(jié)的d值只 作為中間量用于計(jì)算。此算法可有效減少求解的難度和計(jì)算量。
[0053] 常規(guī)算法(2)式在工程應(yīng)用中，當(dāng)已知多自由度臂的各關(guān)節(jié)位置（由關(guān)節(jié)位置傳感 器獲得)和目標(biāo)點(diǎn)在左右眼相機(jī)的圖像坐標(biāo)(U1，V1)和(Ur，Vr)(由相機(jī)圖像處理獲得），就可 W直接進(jìn)行正運(yùn)動學(xué)求解，但卻難W求解逆運(yùn)動學(xué)。而基于虛擬移動關(guān)節(jié)算法中虛擬變量d 無法測量，因此（11)式實(shí)際上無法真正用于求解運(yùn)動學(xué)正解，但（11)式引入了 1個(gè)虛擬變量 d，替代了（2)式中的雙目視覺模型所需的4個(gè)參數(shù)，可有效地利用D-H法進(jìn)行逆運(yùn)動學(xué)求解。 因此，常規(guī)算法用于求解正運(yùn)動學(xué)，基于虛擬移動關(guān)節(jié)算法用于求解逆運(yùn)動學(xué)，兩者結(jié)合則 解決了多自由度雙目視覺系統(tǒng)的目標(biāo)跟蹤控制算法問題。
[0054] 根據(jù)上述模型及算法，可W給出多自由度雙目視覺系統(tǒng)對目標(biāo)跟蹤的控制算法的 流程圖，具體實(shí)施流程如圖3所示，系統(tǒng)先讀取當(dāng)前時(shí)刻t時(shí)各關(guān)節(jié)的01值，W及左眼相機(jī)圖 像的坐標(biāo)值(山，VI)、（Ur, Vr)，代入上方(2)式中，求出當(dāng)前時(shí)刻t時(shí)的正解(XO，y〇, ZO)，再代 入（11)式中求出期望位置下的01值，求取出關(guān)節(jié)控制量A 0。若此時(shí)目標(biāo)處于運(yùn)動狀態(tài)，本 系統(tǒng)會對在每一個(gè)采樣周期A t下重復(fù)上述步驟求取出關(guān)節(jié)控制量A 0，直至A 0 = 0,即目 標(biāo)物靜止時(shí)，停止上述步驟。
[0055] 本領(lǐng)域的普通技術(shù)人員可W理解，上述實(shí)施方式是實(shí)現(xiàn)本發(fā)明的具體實(shí)施例，而 在實(shí)際應(yīng)用中，可W在形式上和細(xì)節(jié)上對其作各種改變，例如將本實(shí)施例中的W左眼相機(jī) 作為基準(zhǔn)替換為W右眼相機(jī)作為基準(zhǔn)替換等有效替換均屬于本發(fā)明的保護(hù)范圍，不偏離本 發(fā)明的精神和范圍。
【主權(quán)項(xiàng)】
1. 一種雙目視覺系統(tǒng)目標(biāo)跟蹤控制方法，其特征在于，包括以下步驟： 51、 通過正運(yùn)動學(xué)取得目標(biāo)點(diǎn)在基坐標(biāo)系中的位置； 52、 根據(jù)所述目標(biāo)點(diǎn)在基坐標(biāo)系中的位置，通過逆運(yùn)動學(xué)取得多自由度機(jī)械臂各關(guān)節(jié) 期望位置； 53、 根據(jù)所述多自由度機(jī)械臂各關(guān)節(jié)期望位置和所述多自由度機(jī)械臂當(dāng)前位置計(jì)算出 所述多自由度機(jī)械臂各關(guān)節(jié)的控制角度； 其中，在求解所述逆運(yùn)動學(xué)中，引入虛擬移動變量d和虛擬坐標(biāo)系{(V }，所述虛擬坐標(biāo) 系從所述基坐標(biāo)系沿光軸方向移動所述d個(gè)單位，使所述虛擬坐標(biāo)系{(V }的原點(diǎn)與目標(biāo)點(diǎn) 重合，通過所述虛擬移動關(guān)節(jié)變量d和所述虛擬坐標(biāo)系{C1 '}利用D-H法計(jì)算出多自由度機(jī) 械臂各關(guān)節(jié)的期望位置。2. 根據(jù)權(quán)利要求1所述的雙目視覺系統(tǒng)目標(biāo)跟蹤控制方法，其特征在于，所述目標(biāo)點(diǎn)在 基坐標(biāo)系中的位置通過D-H法計(jì)算出。3. 根據(jù)權(quán)利要求1所述的雙目視覺系統(tǒng)目標(biāo)跟蹤控制方法，其特征在于，所述目標(biāo)點(diǎn)在 基坐標(biāo)系中的位置的運(yùn)動學(xué)模型為4. 根據(jù)權(quán)利要求1所述的雙目視覺系統(tǒng)目標(biāo)跟蹤控制方法，其特征在于，所述目標(biāo)點(diǎn)在 基坐標(biāo)系中的位置為5. 根據(jù)權(quán)利要求1所述的雙目視覺系統(tǒng)目標(biāo)跟蹤控制方法，其特征在于，所述逆運(yùn)動學(xué) 的求解為n+1自由度機(jī)械臂的逆運(yùn)動學(xué)求解。6. 根據(jù)權(quán)利要求1所述的雙目視覺系統(tǒng)目標(biāo)跟蹤控制方法，其特征在于，所述虛擬移動 關(guān)節(jié)變量d的位置參數(shù)為d = f(xo，yo，zo)。7. 根據(jù)權(quán)利要求1所述的雙目視覺系統(tǒng)目標(biāo)跟蹤控制方法，其特征在于，所述多自由度 機(jī)械臂各關(guān)節(jié)的位置參數(shù)為1 = ;^()，7()，2())，（1 = 1，2，"，11)。8. 根據(jù)權(quán)利要求1所述的雙目視覺系統(tǒng)目標(biāo)跟蹤控制方法，其特征在于，所述正運(yùn)動學(xué) 的正運(yùn)動學(xué)方程為9. 根據(jù)權(quán)利要求8所述的雙目視覺系統(tǒng)目標(biāo)跟蹤控制方法，其特征在于，通過所述正運(yùn) 動學(xué)方程求出所述多自由度機(jī)械臂位于目標(biāo)位置時(shí)的所述多自由度機(jī)械臂各關(guān)節(jié)的位置 參數(shù)θ'：，求出關(guān)節(jié)控制量ΔΘ。10. 根據(jù)權(quán)利要求9所述的雙目視覺系統(tǒng)目標(biāo)跟蹤控制方法，其特征在于，所述目標(biāo)點(diǎn) 處于運(yùn)動狀態(tài)時(shí)，雙目視覺系統(tǒng)能夠根據(jù)所述關(guān)節(jié)控制量A Θ控制所述多自由度機(jī)械臂移 動，直至所述關(guān)節(jié)控制量A Θ = 〇。
【專利摘要】本發(fā)明涉及多自由度雙目視覺系統(tǒng)的運(yùn)動學(xué)建模及目標(biāo)跟蹤控制方法，針對多自由度雙目視覺系統(tǒng)目標(biāo)跟蹤控制任務(wù)，提出了一種的基于虛擬移動關(guān)節(jié)的目標(biāo)跟蹤控制方法。該方法在雙目視覺系統(tǒng)的多自由度臂中引入了一個(gè)虛擬的移動關(guān)節(jié)，通過增加1個(gè)虛擬移動關(guān)節(jié)變量d，替換掉雙目視覺算法中圖像空間的4個(gè)參數(shù)(ul,vl)和(ur,vr)，將n自由度雙目視覺系統(tǒng)的逆運(yùn)動學(xué)求解轉(zhuǎn)化為n+1自由度機(jī)械臂的逆運(yùn)動學(xué)求解，進(jìn)而利用傳統(tǒng)的D-H法進(jìn)行逆運(yùn)動學(xué)求解。本發(fā)明解決了多自由度雙目視覺系統(tǒng)的目標(biāo)跟蹤控制算法問題。本算法具有計(jì)算量小，求解簡單的優(yōu)點(diǎn)，成功應(yīng)用于一個(gè)3自由度的雙目視覺系統(tǒng)的目標(biāo)跟蹤控制任務(wù)。
【IPC分類】G05D3/12, G05B13/04
【公開號】CN105527980
【申請?zhí)枴緾N201510863977
【發(fā)明人】朱俊杰, 李立春, 曹俊生, 孫小珠, 吳金花, 李傳吟, 歐陽文, 査學(xué)雷, 樊蓉, 馮宇, 劉國林
【申請人】上海宇航系統(tǒng)工程研究所
【公開日】2016年4月27日
【申請日】2015年12月1日