雙目視覺系統(tǒng)目標(biāo)跟蹤控制方法
【技術(shù)領(lǐng)域】
[0001 ]本發(fā)明設(shè)及機(jī)器人領(lǐng)域多自由度雙目視覺系統(tǒng)的運(yùn)動(dòng)控制問題，尤其適用于行星 車多自由度雙目視覺系統(tǒng)對(duì)目標(biāo)的跟蹤控制。
【背景技術(shù)】
[0002] 雙目視覺測(cè)量在機(jī)器人領(lǐng)域廣泛應(yīng)用。在航天工程領(lǐng)域，行星車多自由度雙目視 覺系統(tǒng)是一個(gè)應(yīng)用典型，可用于對(duì)目標(biāo)進(jìn)行捜索和跟蹤。一般是將雙目視覺系統(tǒng)安裝在多 自由度臂末端的平臺(tái)上，通過多自由度臂擴(kuò)展雙目視覺相機(jī)的跟蹤范圍。行星車多自由度 雙目視覺系統(tǒng)的目標(biāo)跟蹤任務(wù)可由如圖1所示的模型表示，包括=個(gè)組成部分：多自由度 臂、雙目視覺相機(jī)(包括左眼和右眼)和目標(biāo)。圖中{〇}、和{T}分別表示多自由度臂的基坐標(biāo) 系、左眼相機(jī)坐標(biāo)系和目標(biāo)坐標(biāo)系，UKi = I，2,…，n)表示多自由度臂各關(guān)節(jié)坐標(biāo)系。對(duì)于 行星車多自由度雙目視覺系統(tǒng)的目標(biāo)跟蹤任務(wù)，一般選取目標(biāo)物上的一特征點(diǎn)作為跟蹤目 標(biāo)點(diǎn)，由于相機(jī)鏡頭邊緣崎變的問題，因此希望該目標(biāo)成像點(diǎn)能始終位于相機(jī)圖像的中屯、 (本算法W目標(biāo)成像點(diǎn)位于左眼相機(jī)圖像的中屯、為例）。
[0003] 多自由度雙目視覺系統(tǒng)的目標(biāo)跟蹤控制方法需要建立運(yùn)動(dòng)學(xué)模型：一是正運(yùn)動(dòng)學(xué) 模型，即已知多自由度臂各關(guān)節(jié)U}的位置參數(shù)，求得目標(biāo)坐標(biāo)系{T}在基坐標(biāo)系{0}中的位 姿參數(shù)，由此可獲得目標(biāo)的位置信息;二是逆運(yùn)動(dòng)學(xué)，即已知目標(biāo)坐標(biāo)系{T}在基坐標(biāo)系{0} 中的期望位姿參數(shù)，求得相應(yīng)的多自由度臂各關(guān)節(jié)U}的位置參數(shù)，由此可用于對(duì)多自由度 臂進(jìn)行控制，調(diào)整相機(jī)運(yùn)動(dòng)，實(shí)現(xiàn)對(duì)目標(biāo)跟蹤。
[0004] 目前，常規(guī)的運(yùn)動(dòng)學(xué)算法在求解逆運(yùn)動(dòng)學(xué)時(shí)，需要聯(lián)合基坐標(biāo)系下正運(yùn)動(dòng)學(xué)方程 和圖像坐標(biāo)系下的控制目標(biāo)方程（目標(biāo)點(diǎn)在相機(jī)坐標(biāo)系下X、Y值為0)，求解困難，計(jì)算繁瑣。

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

[0005] 本發(fā)明的目的在于提供計(jì)算方式簡(jiǎn)便的雙目視覺系統(tǒng)目標(biāo)跟蹤控制方法。
[0006] 為解決上述技術(shù)問題，本發(fā)明提供了一種雙目視覺系統(tǒng)目標(biāo)跟蹤控制方法，包括 W下步驟：
[0007] Sl、通過正運(yùn)動(dòng)學(xué)取得目標(biāo)點(diǎn)在基坐標(biāo)系中的位置；
[000引S2、根據(jù)目標(biāo)點(diǎn)在基坐標(biāo)系中的位置，通過逆運(yùn)動(dòng)學(xué)取得多自由度機(jī)械臂各關(guān)節(jié) 期望位置；
[0009] S3、根據(jù)多自由度機(jī)械臂各關(guān)節(jié)期望位置W及多自由度機(jī)械臂當(dāng)前位置計(jì)算出多 自由度機(jī)械臂各關(guān)節(jié)的控制角度；
[0010] 其中，在求解所述逆運(yùn)動(dòng)學(xué)中，引入虛擬移動(dòng)變量d和虛擬坐標(biāo)系促' }，虛擬坐標(biāo) 系{Cl'}從基坐標(biāo)系沿光軸方向移動(dòng)d個(gè)單位，使虛擬坐標(biāo)系{Cl'}的原點(diǎn)與目標(biāo)點(diǎn)重合，通 過虛擬移動(dòng)關(guān)節(jié)變量d和虛擬坐標(biāo)系{打'}利用D-H法計(jì)算出多自由度機(jī)械臂各關(guān)節(jié)的期望 位置。
[0011 ] 其中，D-H法為Denavit和化Kenbe巧于1955年提出了 一種為關(guān)節(jié)鏈中的每一桿件 建立坐標(biāo)系的矩陣方法，運(yùn)種方法在機(jī)器人的每個(gè)連桿上都固定一個(gè)坐標(biāo)系，然后用4X4 的齊次變換矩陣來描述相鄰兩連桿的空間關(guān)系。通過依次變換可最終推導(dǎo)出末端執(zhí)行器相 對(duì)于基坐標(biāo)系的位姿，從而建立機(jī)器人的運(yùn)動(dòng)學(xué)方程。
[0012] 作為優(yōu)選的，目標(biāo)點(diǎn)在基坐標(biāo)系中的位置通過D-取去計(jì)算出。
[0013] 作為優(yōu)選的，目標(biāo)點(diǎn)在基坐標(biāo)系中的位置的運(yùn)動(dòng)學(xué)模型為
[001引作為優(yōu)選的，目標(biāo)點(diǎn)在基坐標(biāo)系中的位置為導(dǎo)戶托,，}^，而,），其中，
[0017]作為優(yōu)選的，逆運(yùn)動(dòng)學(xué)的求解為n+1自由度機(jī)械臂的逆運(yùn)動(dòng)學(xué)求解。
[001引作為優(yōu)選的，虛擬移動(dòng)關(guān)節(jié)變量d的位置參數(shù)為d = f(xo，yo，zo)。
[0019] 作為優(yōu)選的，多自由度機(jī)械臂各關(guān)節(jié)的位置參數(shù)為0i = f(xo，yo，zo)，（i = l，2,…， n) O
[0020] 作為優(yōu)選的，正運(yùn)動(dòng)學(xué)的正運(yùn)動(dòng)學(xué)方程為
[0021 ]辦托於句=妙觸），晚）.…許賄）.於巧。
[0022] 作為優(yōu)選的，通過正運(yùn)動(dòng)學(xué)方程求出多自由度機(jī)械臂位于目標(biāo)位置時(shí)的多自由度 機(jī)械臂各關(guān)節(jié)的位置參數(shù)0 ' 1，求出關(guān)節(jié)控制量A 0。
[0023] 作為優(yōu)選的，目標(biāo)點(diǎn)處于運(yùn)動(dòng)狀態(tài)時(shí)，雙目視覺系統(tǒng)能夠根據(jù)關(guān)節(jié)控制量A 0控制 多自由度機(jī)械臂移動(dòng)，直至關(guān)節(jié)控制量A 0 = 0。
[0024] 本發(fā)明相比現(xiàn)有的雙目視覺系統(tǒng)目標(biāo)跟蹤控制方法，利用虛擬移動(dòng)關(guān)節(jié)變量d，替 換雙目視覺系統(tǒng)目標(biāo)跟蹤控制方法的參數(shù)(Ul，vi)和(Ur, Vr),將n自由度雙目視覺系統(tǒng)的逆 運(yùn)動(dòng)學(xué)求解轉(zhuǎn)化為n+1自由度機(jī)械臂的逆運(yùn)動(dòng)學(xué)求解，運(yùn)樣便可W使用傳統(tǒng)的D-H法進(jìn)行求 解，具有算法巧妙、且計(jì)算量小的特點(diǎn)。
【附圖說明】
[0025] 圖1為本發(fā)明的正運(yùn)動(dòng)學(xué)算法模型示意圖。
[0026] 圖2為本發(fā)明的逆運(yùn)動(dòng)學(xué)算法模型示意圖。
[0027] 圖3為本發(fā)明的目標(biāo)跟蹤算法流程圖。
【具體實(shí)施方式】
[0028] 為使本發(fā)明的目的、技術(shù)方案和優(yōu)點(diǎn)更加清楚，下面將結(jié)合附圖對(duì)本發(fā)明的各實(shí) 施方式進(jìn)行詳細(xì)的闡述。
[0029] 在本發(fā)明的實(shí)施例中，對(duì)圖1中所示的目標(biāo)點(diǎn)位置信息，采用齊次變換可W建立運(yùn) 動(dòng)學(xué)模型，該運(yùn)動(dòng)學(xué)模型為
[0030] 件=巧?鐘。y
[0031] 令目標(biāo)點(diǎn)在基坐標(biāo)系下的位置為^°巧而，y〇，z〇)，.在左眼相機(jī)坐標(biāo)系下的位置為 ;'巧和,，yc,，?)，多自由度臂的各關(guān)節(jié)位置為0l(i = l，2，…，n)，則(l)式可寫成 [003^ 拌柄，折,?) = ';7'的)?訂馬巧
[00削式中，'乃白,）為多自由度臂的連桿變換，i = l，2,…，n;押為左眼相機(jī)相對(duì)多自由 度臂末關(guān)節(jié)In}的位姿。
[0034]目標(biāo)點(diǎn)在左眼相機(jī)坐標(biāo)系下的位置卽的，y(',，z(',），可W通過雙目視覺模型求解， 如W下各式所示：
[003引式中kx，ky，UO，VO為相機(jī)內(nèi)參數(shù)，通過標(biāo)定得到；b為左右眼相機(jī)光軸的間距，且有 6 =馬-? ^ui, VI)和(Ur，Vr)分別為目標(biāo)點(diǎn)在左右眼相機(jī)的圖像坐標(biāo)。
[0039] 因而，可由（2)式求得目標(biāo)點(diǎn)在基坐標(biāo)系下的位置;°p(.vy〇.Z。）。此即為多自由度 雙目視覺系統(tǒng)的正運(yùn)動(dòng)學(xué)。
[0040] 如圖2所示，在左眼相機(jī)光軸方向虛設(shè)一個(gè)移動(dòng)關(guān)節(jié)，左眼相機(jī)坐標(biāo)系{Cl}可W沿 相機(jī)光軸向前移動(dòng)距離d，變?yōu)樽鴺?biāo)系{C'l}，則d為關(guān)節(jié)變量。當(dāng)坐標(biāo)系{C'l}原點(diǎn)移動(dòng)至與 目標(biāo)點(diǎn)（坐標(biāo)系{T}的原點(diǎn)）重合時(shí)，即為滿足目標(biāo)點(diǎn)位于左眼相機(jī)圖像的中屯、。實(shí)現(xiàn)讓右眼 相機(jī)光軸通過目標(biāo)點(diǎn)。
[0041] 因?yàn)閼?少，則（2)式可寫為