徐昱琳， 楊永煥， 李 昕， 陳萬(wàn)米， 晁衍凱

(上海大學(xué)機(jī)電工程與自動(dòng)化學(xué)院，上海200072)

基于雙目視覺(jué)的服務(wù)機(jī)器人仿人機(jī)械臂控制

徐昱琳， 楊永煥， 李 昕， 陳萬(wàn)米， 晁衍凱

(上海大學(xué)機(jī)電工程與自動(dòng)化學(xué)院，上海200072)

機(jī)械臂逆運(yùn)動(dòng)學(xué)、目標(biāo)識(shí)別與定位是服務(wù)機(jī)器人手臂控制中的關(guān)鍵技術(shù)．為了更好地與復(fù)雜多變的非結(jié)構(gòu)化環(huán)境進(jìn)行交互，提出一種應(yīng)用于服務(wù)機(jī)器人平臺(tái)的基于雙目視覺(jué)的仿人機(jī)械臂控制方法．給出一種針對(duì)6自由度手臂的逆解算法，并采用基于雙目視覺(jué)與顏色分割的目標(biāo)識(shí)別方法．然后，根據(jù)識(shí)別出的目標(biāo)三維坐標(biāo)信息控制機(jī)械臂完成特定任務(wù)．本方法在家庭服務(wù)機(jī)器人上得到了驗(yàn)證．

仿人機(jī)械臂;逆運(yùn)動(dòng)學(xué);雙目視覺(jué);HSV;服務(wù)機(jī)器人

Abstract:Inverse kinematics，object localization and manipulation are essential for service robots with manipulators to achieve various human-like tasks．Binocular vision is employed to interact with the environment in which the service robot works．This paper presents an approach for binocular based humanoid manipulation in a service robot system．An inverse kinematic solver is proposed to find all joint angles for a given position of the effectors on the manipulator．The target object is recognized according to segmented colors，and the 3D position computed using the stereo vision system．Having obtained the target position，the manipulator performs a blind grasp．Experimental results show effectiveness of the proposed methods．Key words:humanoid manipulator;inverse kinematics;binocular vision;hue-saturation-value(HSV);service robot

20世紀(jì)60年代初，工業(yè)革命使工業(yè)機(jī)器人進(jìn)入工廠，將人們從危險(xiǎn)和有害的工作中解脫出來(lái)．伴隨著機(jī)電一體化和機(jī)器人技術(shù)的迅猛發(fā)展，越來(lái)越多不同種類的服務(wù)機(jī)器人進(jìn)入人們的生活中，如人形機(jī)器人 ASIMO［1］、廚房機(jī)器人 HRP-2［2］、助殘機(jī)器人 ISAC［3］、圖書管理機(jī)器人 UJI［4］等．先進(jìn)的機(jī)械臂控制對(duì)于服務(wù)機(jī)器人執(zhí)行各種類人任務(wù)是非常重要的，其中機(jī)械臂逆運(yùn)動(dòng)學(xué)和目標(biāo)定位是服務(wù)機(jī)器人機(jī)械臂控制中的關(guān)鍵技術(shù)．

工業(yè)機(jī)器人工作在結(jié)構(gòu)化的環(huán)境中，而服務(wù)機(jī)器人通常面臨一些不確定的、非結(jié)構(gòu)化的和復(fù)雜的環(huán)境［5］，工作在這種環(huán)境下的服務(wù)機(jī)器人必須實(shí)時(shí)地與外部環(huán)境以及環(huán)境中的其他機(jī)器人和人進(jìn)行交互．為了使機(jī)械臂具有自主地從未知環(huán)境中獲取信息的能力，需要給機(jī)械臂配備各種外部傳感器，如觸覺(jué)、距離和視覺(jué)傳感器等，其中視覺(jué)傳感器具有信號(hào)范圍大、信息完整等特點(diǎn)，被認(rèn)為是最重要的傳感器．雙目視覺(jué)傳感器由于可以直接獲取物體的三維坐標(biāo)信息，為機(jī)械臂的控制帶來(lái)了很大的方便，因此基于雙目視覺(jué)的機(jī)械臂控制受到國(guó)內(nèi)外研究者的廣泛關(guān)注．斯坦福大學(xué)的 STAIR［6］(Stanford AI robot)是一個(gè)具有5自由度(degrees of freedom，DOF)機(jī)械臂的人工智能研究平臺(tái)，通過(guò)安裝在臂末端的雙目攝像頭，可以控制機(jī)器人拿取一件事先未知具體形狀的物體．德國(guó)航空航天中心研制的機(jī)器人Robutler［7］是一個(gè)可移動(dòng)式平臺(tái)，其具備固定的立體攝像系統(tǒng)、激光掃描儀和一個(gè)安裝在平臺(tái)上的7自由度手臂，該機(jī)械臂的視覺(jué)伺服系統(tǒng)采用的是手眼(eye-in-hand)系統(tǒng)．

此外，機(jī)械臂的逆運(yùn)動(dòng)學(xué)算法也是機(jī)械手控制研究中的重要部分之一．機(jī)械手運(yùn)動(dòng)學(xué)逆解是指通過(guò)已知末端執(zhí)行器的坐標(biāo)位置或位姿來(lái)計(jì)算機(jī)械手各個(gè)關(guān)節(jié)的角度值［8］．機(jī)械臂的逆解問(wèn)題最終一般都?xì)w結(jié)為求解非線性方程組的問(wèn)題．非線性方程組的求解方法有很多，主要包括數(shù)值方法和代數(shù)方法．常用的數(shù)值方法主要有牛頓拉夫森法、優(yōu)化算法、遺傳算法等;常用的代數(shù)方法主要有析配消元法、聚篩法、Groebner基法和吳文俊消元法．本工作提出了一種求解6自由度仿人機(jī)械臂逆解的算法，采用基于雙目視覺(jué)與顏色分割的目標(biāo)識(shí)別方法，根據(jù)識(shí)別出的目標(biāo)三維坐標(biāo)信息控制機(jī)械臂完成特定任務(wù)．本方法在家庭服務(wù)機(jī)器人上得到了驗(yàn)證．

1 機(jī)械臂數(shù)學(xué)模型的建立

機(jī)械臂數(shù)學(xué)模型的建立是機(jī)械臂運(yùn)動(dòng)學(xué)中的重要部分．本系統(tǒng)采用6自由度仿人機(jī)械臂，如圖1所示，肩部、肘部與腕部各有2個(gè)旋轉(zhuǎn)自由度．本工作采用D-H(denavit-hartenberg)方法建立機(jī)械臂的數(shù)學(xué)模型［9］，機(jī)械臂的參考坐標(biāo)系如圖2所示．根據(jù)D-H法可以得出本機(jī)械臂的D-H參數(shù)，如表1所示．

圖1 6自由度仿人機(jī)械臂Fig．1 6-DOF manipulator of the robot

圖2 仿人機(jī)械臂各關(guān)節(jié)參考坐標(biāo)系Fig．2 Reference frames for the humanoid robot arm

表1 D-H參數(shù)Table 1 D-H parameters

表1中，a表示相鄰兩個(gè)關(guān)節(jié)軸線方向的公垂線的長(zhǎng)度(關(guān)節(jié)偏移);α表示兩個(gè)相鄰z軸之間的角度(關(guān)節(jié)扭轉(zhuǎn));d表示在z軸上的兩條相鄰的公垂線之間的距離;θ表示繞 z軸的旋轉(zhuǎn)角．矩陣Ai(i=1，2，…，6)為后一個(gè)關(guān)節(jié)變換到前一個(gè)關(guān)節(jié)的轉(zhuǎn)換矩陣:

通過(guò)從參數(shù)表中選取參數(shù)帶入上述矩陣，便可以寫出每?jī)蓚€(gè)相鄰關(guān)節(jié)之間的變換．已知各關(guān)節(jié)轉(zhuǎn)角，通過(guò)關(guān)節(jié)間的位姿矩陣可以求得機(jī)器人手臂運(yùn)動(dòng)學(xué)正解為

式中，p=? px，py，pz」為手臂末端執(zhí)行器位置矢量;n，o，a為手臂末端執(zhí)行器的姿態(tài)矢量，其中n=?nx，ny，nz」為平行于夾持器平面的單位矢量，o=? ox，oy，oz」為垂直于夾持器平面的單位矢量，a=? ax，ay，az」為指向目標(biāo)物體的接近矢量．

2 手臂運(yùn)動(dòng)學(xué)逆解

逆運(yùn)動(dòng)學(xué)問(wèn)題在機(jī)器人運(yùn)動(dòng)學(xué)、動(dòng)力學(xué)及控制的研究中占有重要的地位，直接影響著控制的準(zhǔn)確性與快速性．逆運(yùn)動(dòng)學(xué)問(wèn)題就是根據(jù)已知的末端執(zhí)行器的位姿位置和姿態(tài)，求解相應(yīng)的關(guān)節(jié)變量．本機(jī)械臂是模仿人的手臂設(shè)計(jì)的，其對(duì)應(yīng)的簡(jiǎn)化模型如圖3所示．設(shè)機(jī)械臂末端位置和姿態(tài)矩陣為

本逆解算法由以下4個(gè)步驟解得．

步驟(1):關(guān)節(jié)5在手抓坐標(biāo)系x6y6z6下的坐標(biāo)為(0，0，-l5)，其在基礎(chǔ)坐標(biāo)系下的坐標(biāo)為(x5，y5，z5)，則

圖3 手臂模型等效簡(jiǎn)化圖Fig．3 Simplified arm model

3 目標(biāo)識(shí)別與定位

對(duì)基于視覺(jué)的機(jī)械臂控制來(lái)說(shuō)，機(jī)器人視覺(jué)是一個(gè)非常重要的組成部分．機(jī)器人只有在視覺(jué)的指引之下，才能獲得目標(biāo)的正確信息，從而控制機(jī)械臂完成指定的任務(wù)．視覺(jué)系統(tǒng)必須在標(biāo)定的基礎(chǔ)上，識(shí)別和跟蹤目標(biāo)對(duì)象的位置和姿態(tài)．在圖像中，識(shí)別目標(biāo)對(duì)象可以基于3個(gè)不同的屬性［10］，即顏色、紋理和形狀．識(shí)別出目標(biāo)物體之后，即可根據(jù)雙目攝像頭計(jì)算出目標(biāo)物體的三維坐標(biāo)值．顏色特性是應(yīng)用最廣泛、最容易學(xué)習(xí)與實(shí)現(xiàn)的方式，因此，本工作采用基于顏色的物體識(shí)別方式．

本系統(tǒng)采用加拿大Point Grey公司的Bumblebee2作為雙目視覺(jué)傳感器，通過(guò)提取目標(biāo)物體的顏色信息進(jìn)行HSV(hue-saturation-value)閾值分割，得到目標(biāo)物體的三維坐標(biāo)．

3．1 雙目立體視覺(jué)模型

本立體視覺(jué)系統(tǒng)基于平行光軸理論［11］(見(jiàn)圖4)，其中 b表示基線距離，f為攝像頭的焦距，M(xm，ym，zm)為目標(biāo)物體在世界坐標(biāo)系的坐標(biāo)值，I1與I2為左右兩個(gè)攝像頭的成像平面．

根據(jù)△MI1I2與△MOF2的相似關(guān)系，可以得到目標(biāo)物體在世界坐標(biāo)系中的坐標(biāo)值為

圖4 攝像頭與成像平面的幾何關(guān)系Fig．4 Geometric relationship between cameras and image planes

3．2 顏色空間轉(zhuǎn)換

采用彩色圖像分割的方法識(shí)別目標(biāo)時(shí)，要選擇合適的顏色空間，常見(jiàn)的顏色空間有 RGB(redgreen-blue)，HSV，CMY(cyan-magenta-yellow)等．RGB是常見(jiàn)的顏色空間，一般CCD(charge-coupled device)攝像機(jī)輸出用RGB顏色空間表示，若選擇RGB顏色空間，則無(wú)須轉(zhuǎn)換．但在實(shí)際應(yīng)用中，由于RGB顏色空間受光源的種類、光照的強(qiáng)度等因素影響，同一顏色屬性的物體測(cè)出的RGB值分布很分散，3個(gè)分量互相關(guān)聯(lián)變化，很難確定識(shí)別RGB的閾值范圍，容易把并非指定顏色的物體包含進(jìn)去，或漏掉應(yīng)該識(shí)別的部分物體．

而HSV模型可以在一定程度上避免RGB模型的高分散性和高相關(guān)性所帶來(lái)的閾值劃分問(wèn)題．HSV顏色模型反映了人眼觀察彩色的視覺(jué)規(guī)律，在實(shí)際中應(yīng)用較多．從RGB到HSV的轉(zhuǎn)化過(guò)程相當(dāng)于一個(gè)解耦的工作．在HSV空間中，色調(diào)H(hue)反映了該顏色最接近何種可見(jiàn)光譜波長(zhǎng)，即某一種顏色．色調(diào)H用角度來(lái)表示，一般規(guī)定0°的彩色為紅色，120°的為綠色，240°的為藍(lán)色，色調(diào)從 0°～ 360°覆蓋了所有的彩色．飽和度S(saturation)表示一種顏色相對(duì)于其純色的比例，即純潔性，可用來(lái)區(qū)別顏色明暗的程度．完全飽和的顏色是指由單一波長(zhǎng)組成的光譜色，當(dāng)一種顏色摻入其他光成分愈多時(shí)，顏色愈不飽和．飽和度在顏色圓上用半徑表示，沿徑向方向上的不同顏色具有相同的色調(diào)和亮度，但飽和度不同．V(value)表示色彩的明亮程度，范圍為0～1，一個(gè)極端是黑色(沒(méi)有光)，另一個(gè)極端是白色，在這兩個(gè)極端之間是不同明暗程度的灰色．在圓柱體中常用垂直軸表示，底部的亮度最小，頂部的亮度最大．本研究采用HSV顏色空間進(jìn)行目標(biāo)識(shí)別，HSV的顏色模型［12］如圖5所示，其中 RGB到 HSV的轉(zhuǎn)換公式為

圖5 HSV顏色空間Fig．5 Color space of HSV

3．3 目標(biāo)定位

首先，將每一幀圖像的像素點(diǎn)轉(zhuǎn)換成HSV值，通過(guò)離線學(xué)習(xí)并記錄目標(biāo)物體的HSV閾值信息．程序?qū)崟r(shí)運(yùn)行時(shí)，提取閾值信息得到目標(biāo)區(qū)域后，通過(guò)雙目視覺(jué)計(jì)算得到目標(biāo)的三維坐標(biāo)信息．本系統(tǒng)基于VC++6．0軟件平臺(tái)，圖6為程序運(yùn)行界面，左下角為物體的三維坐標(biāo)值．

圖6 程序調(diào)試界面(離線模式)Fig．6 Program debugging interface(off-line mode)

4 實(shí)驗(yàn)

本工作提出了一種基于雙目視覺(jué)的仿人機(jī)械臂控制方法，為了驗(yàn)證逆解求解以及顏色濾波方法的正確性，將本方法應(yīng)用于家庭服務(wù)機(jī)器人平臺(tái)，控制服務(wù)機(jī)器人完成自主抓取任務(wù)．

4．1 服務(wù)機(jī)器人硬件平臺(tái)介紹

本系統(tǒng)中的家庭服務(wù)機(jī)器人實(shí)驗(yàn)平臺(tái)如圖7所示，由2自由度頭部和2個(gè)6自由度仿人機(jī)械臂組成，底部采用差動(dòng)輪式結(jié)構(gòu)運(yùn)動(dòng)，機(jī)器人周圍分布著8路光電傳感器和8路超聲波傳感器．機(jī)器人主控制器使用一臺(tái)NISE3110工控機(jī)，固定在機(jī)器人腹部．機(jī)器人雙臂電機(jī)以及底輪電機(jī)通過(guò)CAN總線與工控機(jī)進(jìn)行通信．

圖7 實(shí)驗(yàn)平臺(tái)Fig．7 Experimental setup

4．2 實(shí) 驗(yàn)

本實(shí)驗(yàn)設(shè)計(jì)機(jī)器人自主抓取，通過(guò)分析目標(biāo)物體實(shí)際坐標(biāo)位置與手爪到達(dá)位置之間的誤差值，從而證明逆解算法與顏色濾波算法的正確性．

目標(biāo)物體為綠色瓶子，保證目標(biāo)物體在世界坐標(biāo)系中的位置不變．機(jī)器人的位置是變化的，每一次抓取時(shí)機(jī)器人的位置都是不同的，但需要保證目標(biāo)物體在機(jī)器人手臂的抓取范圍內(nèi)．立體視覺(jué)系統(tǒng)處理圖像的速度為15幀/s，每一幀圖像的分辨率為640×480．實(shí)驗(yàn)數(shù)據(jù)如表2所示，其中3個(gè)坐標(biāo)值的參考坐標(biāo)系均為機(jī)器人右肩關(guān)節(jié)坐標(biāo)系，單位為mm．目標(biāo)物體的實(shí)際測(cè)量位置是指目標(biāo)物體在機(jī)器人右肩關(guān)節(jié)坐標(biāo)系中的坐標(biāo)值，雙目計(jì)算值為通過(guò)顏色分割算法求出的目標(biāo)物體坐標(biāo)，手臂末端夾持器位置值為通過(guò)顏色濾波得到的位置值求取手臂逆解后手臂運(yùn)動(dòng)到的位置，誤差值為實(shí)際測(cè)量值與夾持器位置之間的差值．

實(shí)驗(yàn)得出，機(jī)器人計(jì)算得到的位置與目標(biāo)物體實(shí)際位置之間的平均誤差為(20．25，1．42，0．15)．目標(biāo)物體高度為15 cm，直徑為6 cm，因此，實(shí)驗(yàn)誤差在允許誤差范圍內(nèi)，機(jī)器人可以完成自主抓取任務(wù)，本工作中的方法得到了驗(yàn)證．機(jī)械臂抓取流程如圖8所示．

圖8 仿人機(jī)械臂抓取動(dòng)作Fig．8 Grasping of humanoid manipulator

表2 實(shí)驗(yàn)數(shù)據(jù)Table 2 Experimental results

5 結(jié)束語(yǔ)

本工作基于服務(wù)機(jī)器人復(fù)雜多變的非結(jié)構(gòu)化工作環(huán)境，提出了一種基于雙目視覺(jué)的仿人機(jī)械臂控制方法．首先，給出了一種針對(duì)6自由度手臂的逆解算法，采用基于雙目視覺(jué)與顏色分割的目標(biāo)識(shí)別方法;然后，根據(jù)識(shí)別出的目標(biāo)三維坐標(biāo)信息控制機(jī)械臂完成特定任務(wù);最后，通過(guò)在人形機(jī)器人平臺(tái)上的應(yīng)用，控制機(jī)器人完成自主抓取任務(wù)，使本方法得到驗(yàn)證．本研究中的雙目視覺(jué)系統(tǒng)穩(wěn)定準(zhǔn)確，能夠?qū)崟r(shí)得到目標(biāo)物體的位置與姿態(tài)，滿足抓取要求，實(shí)現(xiàn)了服務(wù)機(jī)器人非結(jié)構(gòu)化環(huán)境下的抓取操作．今后的工作主要在圖像處理速度、處理算法上進(jìn)行改進(jìn)與提高，使系統(tǒng)實(shí)時(shí)性更高，提高系統(tǒng)的定位精度．

［1］ SAKAGAMI Y，WATANABE R，AOYAMA C，et al．The intelligent ASIMO:System overview and integration［C］∥Proceeding of the 2002 IEEE/RSJ International Conference on Intecligent Robots and Systems．Saitama:Honda R＆D Co．Ltd．，2002:2478-2483．

［2］ HARA I，ASANO F，ASOH H，et al．Robust speech interface based on audio and video information fusion for humanoid HRP-2 ［J］．Intelligent Robots and Systems，2004:2404-2410．

［3］ KAWAMURA K，PETERS II R A，WILKES D M，et al．ISAC:Foundations in human-humanoid interaction ［J］．IEEE Intelligent System，2000，15(4):38-45．

［4］ PRATS M，SANZ P J，DEL POBIL A P．Model-based tracking and hybrid force/vision control for the UJI librarian robot［C］∥Intelligent Robots and Systems，IEEE/RSJ International Conference on Digital Object Identifier．2005．

［5］ KEMP CC，EDSINGER A，TORRES-JARA E．Challenges for robot manipulation in human environments:Developing robots that perform useful work in everyday settings［J］．IEEE Robotics and Automation Society，2007:20-29．

［6］ ASHUTOSH S，JUSTIN D，JUSTIN K，et al．Robotic grasping of novel objects using vision［J］．International Journal of Robotics Research，2008:157-173．

［7］ HILLENBRAND U，BRUNNER B，BORST C，et al．The robutler:A vision-controlled hand-arm system for manipulating bottles and glasses［C］∥ISR 2004 35th International Symposium on Robotics，Paris-Nord Villepinte Exhibition Centre．2004．

［8］ 范叔炬．機(jī)械臂運(yùn)動(dòng)學(xué)算法設(shè)計(jì)［D］．杭州:浙江大學(xué)，2008:42．

［9］ NIKU S B．Introduction to robotics analysis，systems，applications［M］． Beijing:Publishing House of Electronics Industry，2004:28-85．

［10］ JAN P． Vision based mobile manipulation ［D］．Regensburg:Regensburg University of Applied Sciences，2008．

［11］ LEE H J，LEE M C．Technique for localization and visual servoing of mobile manipulators［J］．IEEE International Symposium on Industrial Electronics，2009．

［12］ SMITH A R．Color gamut transform pairs ［C］∥Computer Graphics(SIGGRAPH)．1978:12-19．

Binocular Vision-Based Humanoid Manipulator Control for Service Robot

XU Yu-lin， YANG Yong-huan， LI Xin， CHEN Wan-mi， CHAO Yan-kai
(School of Mechatronics Engineering and Automation，Shanghai University，Shanghai 200072，China)

TP 24

A

1007-2861(2012)05-0506-07

10．3969/j．issn．1007-2861．2012．05．013

2011-03-12

徐昱琳(1964～)，女，副教授，博士，研究方向?yàn)闄C(jī)器人智能控制．E-mail:xuyulin@shu．edu．cn