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

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

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