基于姿態(tài)閉環(huán)控制的球面并聯(lián)仿生眼系統(tǒng)設(shè)計與研究.doc

基于姿態(tài)閉環(huán)控制的球面并聯(lián)仿生眼系統(tǒng)設(shè)計與研究第33卷第3期2011年5月機器人ROBOT)1.33.No.3May,2011DoI:1O.3724/SEJ.1218.2011.00354基于姿態(tài)閉環(huán)控制的球面并聯(lián)仿生眼系統(tǒng)設(shè)計與研究李超,謝少榮,李恒宇,繆金松,徐元玉,羅均(上海_人學(xué)機電工程與自動化學(xué)院,二海200072)摘要:提出了一種基于姿態(tài)反饋的球面并聯(lián)機構(gòu)閉環(huán)控制方法,有效地解決了該機構(gòu)輸入與輸出之間復(fù)雜的3維非線性強耦合映射關(guān)系給構(gòu)建閉環(huán)系統(tǒng)帶來的問題.問時將球面并聯(lián)機構(gòu)用到機器人眼設(shè)計上,制作了類似人跟運動特點,比人眼尺寸略大,具有控制和視頻信號接口的仿生眼實物:基于數(shù)字信號處理器(DSP)的控制系統(tǒng)實現(xiàn)了逆解和閉環(huán)控制算法的實時在線計算.實驗結(jié)果表明,姿態(tài)閉環(huán)控制算法有效提高了仿生眼的定位精度.關(guān)鍵詞:球面并聯(lián)機構(gòu);機器人眼;仿生眼;閉環(huán)控制,中圖分類號:TP273文獻標(biāo)識碼:A文章編號:10020446(2011)一03035406SystemDesignandStudyonBionicEyeofSphericalParallelMechanismBasedonAttitudeClosed-loopControlLIChao,XIEShaorong,LIHengyu,MIAOJinsong,XUYuanyu,LUOJun(SchoolofMechatronicsEngineeringandAutomation,ShanghaiUniversity,Shanghai200072,China)Abstract:Anattitudefeedbackbasedmethodforestablishingtheclosedloopcontrolsystemofsphericalparallelmechanism(SPM)isproposedtocircumventthedifficultycausedbythecomplex,threedimensional,nonlinearandstronglycoupledrelationshipbetweentheinputandoutputofthemechanism.SPM,then,isemployedtOthedesignofbioniceyewhichemulatesthefunctionofhumanSeyebutisbiggerthanitinsize,andincorporatestheinterfaceofthesignalofcontrolandvideo.Inaddition,therealtimeonlinecalculationoftheinversekinematicsandtheschemeoftheclosed-loopcontrolisconductedonDSP(digitalsignalprocessor).Finally,theexperimentalresultssubstantiallyconfirmthattheimprovedpositioningprecisionofthebioniceyeisobtainedbyintroducingtheproposedalgorithm.Keywords:sphericalparallelmechanism(SPM);roboteye;bioniceye;closedloopcontroll引言(Introduction)球面并聯(lián)機構(gòu)是一種可實現(xiàn)三自由度轉(zhuǎn)動的l并聯(lián)機構(gòu)I1j,這種運動特點正好與人眼的運動特點(.?相同.參考人眼運動特點和結(jié)構(gòu)特點設(shè)計的球面并聯(lián)仿生眼,彌補了當(dāng)前絕大部分機器人眼只能實現(xiàn)二自由度轉(zhuǎn)動而缺少繞視軸自旋的缺憾,降低了機器人自身姿態(tài)改變和目標(biāo)本體姿態(tài)變化對接收視頻圖像質(zhì)量的影響,減輕了人眼觀察和計算機處理這些視頻信號的難度.在仿生眼三維模型的運動仿真結(jié)果與理論計算結(jié)果完全一致后,構(gòu)建了基于DSP的控制系統(tǒng)對仿生眼進行運動控制.但在此過程中,我們發(fā)現(xiàn)由于各零件尺寸很小,結(jié)構(gòu)復(fù)雜且空間尺寸多等岡素影響,零件的加工精度和裝配精度等很難滿足設(shè)計要求,仿生眼有一定的定位誤差,尤其是在大角度定位時.為了解決該問題,本文提出一種基于姿態(tài)反饋的球面并聯(lián)機構(gòu)閉環(huán)控制方法,通過在眼球上安裝姿態(tài)傳感器反饋實時姿態(tài),有效地解決了球面并聯(lián)機構(gòu)輸入與輸出之間復(fù)雜的3維非線性強耦合映射關(guān)系給構(gòu)建閉環(huán)系統(tǒng)帶來的諸多問題,提高了仿生眼的定位精度.2眼球運動特點及Fick坐標(biāo)系(Movingcharacteristicsofeyeandcoordinatesys.temsofFick)眼球運動在視覺中起著至關(guān)重要的作用,它可以近似地看作是眼球繞其幾何中心的3自由度球面運動I3】.它由6塊眼外肌控制,眼外肌的伸縮能使眼球向各個方向轉(zhuǎn)動,這些伸縮是在大腦調(diào)節(jié)中樞控制之下完成的.每眼各有6條眼外肌,如圖1所基金項目:國家863計劃資助項目(2007AA04Z225,2009AA04Z211);國家自然科學(xué)基金資助項目(60975068)通訊作者:李超,1收稿/錄用/修同:20100913/2010.11-03/2011-0223第33卷第3期李超等:基于姿態(tài)閉環(huán)控制的球面并聯(lián)仿生眼系統(tǒng)設(shè)計與研究355示.在眼外肌的驅(qū)動下,通常眼球上下和左轉(zhuǎn)動角度可達5:38.,白旋角可達15.通過眼球旋轉(zhuǎn)中心設(shè)有3個假想的軸,即Fick三軸,此3軸相互垂直構(gòu)成Fick坐標(biāo)系2_水平軸(1,軸),通過兩眼旋轉(zhuǎn)中心的假想軸,圍繞此軸眼球能作上下轉(zhuǎn)動;垂直軸(x軸),通過眼球旋轉(zhuǎn)中心的一根從上向并與水平軸垂直的假想軸,圍繞此軸眼球能作左右轉(zhuǎn)動;前后軸(z軸),通過眼球旋轉(zhuǎn)中心并與水平軸和垂直軸成正交的一根從前向后的假想軸,同繞此軸眼球能作內(nèi)外側(cè)旋;Listing平面,假想的通過眼球旋轉(zhuǎn)中心和水平軸以及垂軸的平面4J.Z1眼外肌和Fick坐標(biāo)系Fig.1ExtraocularmuscleandcordinatesystemsofFickZ3球面并聯(lián)仿生眼設(shè)計及姿態(tài)逆解(DesignofbioniceyeofSPMandattitudecalcula.tionofinversekinematics)3.1球面并聯(lián)仿生眼運動特點球面并聯(lián)機構(gòu)結(jié)構(gòu)簡圖如圖2所示,它由上平臺,底座和3對由下連桿與上連桿構(gòu)成的支鏈組成.同連在底庫上的電機與下連桿,下連桿與上連桿,上平臺與上連桿均由轉(zhuǎn)動副連接,9個轉(zhuǎn)動副軸線相交丁0點,該點稱為機構(gòu)的轉(zhuǎn)動中心,過D點并與底座平行的平面稱為球心平面1,5-7.在3個電機的聯(lián)合驅(qū)動卜,上平臺相對底座可實現(xiàn)3白由度旋轉(zhuǎn).該運動特點恰好與眼球運動特點相同,且它們的外形利驅(qū)動形式都十分相似,故將球面并聯(lián)機構(gòu)府用在仿生眼設(shè)計上.機構(gòu)的球心平面看作是眼球的Listing平面,以球心D為原點建立的固定坐標(biāo)系D一看作是眼球的Fick坐標(biāo)系.球面并聯(lián)機構(gòu)共有5個結(jié)構(gòu)參數(shù):,】,77,其中,分別為下連桿和上連桿的結(jié)構(gòu)夾角;盧,盧2分別為底座和上平臺的半錐角;叼為初始狀態(tài)時上平臺和底座的結(jié)構(gòu)扭角.參考人眼的結(jié)構(gòu)特點優(yōu)選結(jié)構(gòu)參數(shù)(=60.,a2=60.,1=3O.,=130.,77=127.)進行設(shè)計,根據(jù)該參數(shù)設(shè)計的仿生眼在外形和運動范罔上都與人眼相近,圖3為根據(jù)這些參數(shù)制作的仿生眼實物,其眼球半徑22.5mm,最大截面半徑50mm.圖2球面并聯(lián)機構(gòu)結(jié)構(gòu)圖Fig.2ThestructuresofSPM圖3仿生眼實物Fig.3BioniceyeofSPM3.2仿生眼姿態(tài)逆解如圖2,以球心D為原點建立定坐標(biāo)系D一,動坐標(biāo)系O-XYZ和連桿坐標(biāo)系0一ff(,J=1,2,3).Zo軸為底座中心H與原點D的連線,正方向指向上平臺;z軸為原點D與上平臺中心的連線,正方向指向上平臺;,軸為第個分支的第.,個轉(zhuǎn)動副方向,正方向指向球外;Xo軸方向為機器人2011年5月軸和zl1軸所成平面的法線方向;x軸方向為Z0軸和z13軸所成平面的法線方向;,軸方向為由與川軸所成平面的法線方向;各系j,軸方向由右手法則確定.眼球相對于底座只有轉(zhuǎn)動,眼球姿態(tài)可由動坐標(biāo)系O-XYZ相對同定坐標(biāo)系D一Zn的方向余弦矩陣Rxyz來表示5-81.Jyz=c加cssc一cscs妒zssCrs+cc一ssccss妒ZccIccl式中,加,分別表示眼球的左右轉(zhuǎn)角,俯仰角,自旋角,s,c分別表示sin,COS.wi定義向量:|沿】方向,1,f沿Z2方向,w,沿3方向.通過何姿變換求出向量蜥,在固定坐標(biāo)系D一z=0卜的方向矢量.一一c+vFJss3ccc一c一44(3)式中叩1l=0.,rh2=120.,rh3=240.,(i=1,2,3)為3個電機的轉(zhuǎn)角.同樣通過位姿變換可以求出向量在動坐標(biāo)系0一XYZ下的方向矢量.s130.s%c加c+s130.C172iCs一s130.C2iS鋤s0c0z+c130.ss妒Z+c130.c0xsc鉈s130.srh;cs一s130.C2;Cc一s130.crhssc一c130.sc+c130.cssCz-s130.sr/2s一s130.C2iSc+c130.cc式中rhl=127.,叩22=247.,叩23=7.,分別表示眼球3個姿態(tài)角.利用向量與向量w的夾角為,建立約束方程4-81:,Mf=0.5,f=1,2,3(5)整理:Afsin0/+BiCOSOi+C/=0(6)解方程,得到:=2arctan(7)或=2arctan22(8)式中Af,Bf,G由,求出,根據(jù)圖3的裝配形式,式(8)為仿生眼的姿態(tài)逆解,為了簡化表達,用下式表示:=,R(,)(9)4控制系統(tǒng)設(shè)計(Designofcontrolsystem)4.1硬件設(shè)計求解仿生眼姿態(tài)逆解過程繁瑣,需要大量的三角函數(shù)和反三角函數(shù)等復(fù)雜函數(shù)計算,且逆解需要在線實時計算,這對控制器的運算速度和運算精(4)度要求很高.根據(jù)這些要求,控制器選用TMS320F2812型號DSP,它特別適用于有大量數(shù)據(jù)處理的控制場合,運行時鐘達150MHz,且引導(dǎo)ROM保存了標(biāo)準的數(shù)學(xué)函數(shù)表,可以方便地進行各種函數(shù)的調(diào)用_9J.姿態(tài)傳感器選用美國GladiatorTechnologies公司的LandMarkl0AHRS全硅6自由度數(shù)字姿態(tài)和航向參考系統(tǒng)(AHRS).它提供RS485輸出,包括deltavelocity,deltatheta,水平轉(zhuǎn)角,俯仰角和自旋角.角度靜態(tài)精度可達0.2.,動態(tài)精度0.5.,采樣頻率可達200Hz,這些性能滿足系統(tǒng)需要.為保證仿生眼能夠穩(wěn)定高效地工作,采用圖4所示的結(jié)構(gòu)進行系統(tǒng)硬件設(shè)計,并利用LabVlEW編寫上位機(圖5)對眼球姿態(tài)進行控制,對整個系統(tǒng)進行監(jiān)控和數(shù)據(jù)分析.4.2姿態(tài)閉環(huán)控制算法研究在實際控制過程中,由于零件加工精度和裝配精度等因素影響,仿生眼的定位精度較差,尤其在入角度轉(zhuǎn)動時(火于等于30.).為提高系統(tǒng)精度,在眼球上安裝LandMarklOAHRS(圖6),利用它對眼球姿態(tài)的實時測量與反饋,對姿態(tài)進行閉環(huán)控制.由于仿生眼3個姿態(tài)角和3個電機轉(zhuǎn)角之間是一種復(fù)雜的3維非線性強耦合映射關(guān)系,每個電機轉(zhuǎn)角都由3個姿態(tài)角共同確定,因此傳統(tǒng)的單軸位置閉環(huán)控制算法在仿生眼的姿態(tài)閉環(huán)控制上已不適用,更不能簡單地根據(jù)姿態(tài)誤差直接對眼球姿態(tài)進第33卷第3期李超等:基于姿態(tài)閉環(huán)控制的球商并聯(lián)仿生眼系統(tǒng)設(shè)計與研究357一一一,/一一一一一一,圖4硬件體系Fig.4Hardwarearchitectures圖5監(jiān)摔界面Fig.5Monitorinterface圖6仿生眼和姿態(tài)傳感器Fig.6Bioniceyeandattitudetransducer行補償.要通過姿態(tài)誤差對眼球姿態(tài)進行準確的補償,必須對姿態(tài)誤差產(chǎn)生的原岡進行分析.該系統(tǒng)中,姿態(tài)輸入值與姿態(tài)實際值之間的姿態(tài)誤差是由各零件加l精度,裝配精度,電機定位精度,傳感器精度以及隨機誤差等岡素共同決定,其中前3個岡素可以歸結(jié)為結(jié)構(gòu)參數(shù)的實際值與設(shè)計值之間的偏著,這里的結(jié)構(gòu)參數(shù)是0f,盧,這5個參數(shù)在3個支鏈中各自的實際值.受加L:精度等岡素影響,它們并不是精確地對稱相等.而式(9)的姿態(tài)逆解模型,是建立在各尺寸都對稱相等,且都與設(shè)計值相同的理想情況下,因此式(9)是對實際姿態(tài)逆解的近似表示,需要盡可能利用其他信息對式(9)的近似表示進行完善,使系統(tǒng)能夠更準確地進行定位.該系統(tǒng)中,可利用的信息有姿態(tài)誤差和姿態(tài)實測值.將式(9)的逆解模型作為算法的核心,通過姿態(tài)誤差和姿態(tài)實測值對姿態(tài)進行補償,建立7所示的姿態(tài)閉環(huán)控制算法./.,I雖I=fR()=(+-廠M()+(T)(10)/式中,=ll為3個姿態(tài)角輸入值,I量=l00203l為3個電機轉(zhuǎn)角輸出值,=y為姿態(tài)測量值,=為姿態(tài)偏差,=+十為姿態(tài)校正后輸入值,.為逆解模型,廠M為姿態(tài)偏差值補償函數(shù),.為姿態(tài)實測值補償函數(shù).圖7姿態(tài)閉環(huán)控制原理圖Fig.7Schematicofattitudeclosedloopcontrol358機器人2011年5月姿態(tài)偏差值補償函數(shù).由于系統(tǒng)非線性強耦合特征,單方向偏差補償跟3個方向姿態(tài)誤差以及其他因素都有密切關(guān)系,因此很難總結(jié)出一個準確的偏差補償函數(shù).為簡化系統(tǒng)設(shè)計難度,假設(shè)單方向偏差補償函數(shù)只由該方向的誤差決定,采用次項方程近似表示兩者關(guān)系:axayaz+bx000by000bzCX000Cy000CZ()()().+式中,ax,ay,az為常數(shù),bbbz為一次項系數(shù),CX,Ccz為二次項系數(shù).姿態(tài)實測值補償函數(shù),N.參考l廠M,同樣假設(shè)單方向姿態(tài)實測值補償函數(shù)只由該方向姿態(tài)實測值決定,采用二次項方程近似表示兩者關(guān)系:(痧yrZ+bTx000bTy000bTz00CTy00CTZ婊加x衍yz+(12)式中,nTy,為常數(shù),bTbTy,bTz為一次項4020O_204O系數(shù),CTX,cTCTz為二次項系數(shù).在調(diào)試過程中,根據(jù)實際控制效果,對式(1o)和(11)中各常數(shù)和系數(shù)不斷地進行調(diào)整,以達到最佳的定位精度,系統(tǒng)采用ax=0.3,ay=一0.2,az=0,bx=1.1,br=1.2,bz:0.2,CX=0.0002,Cy=0.0003,CZ=0,aTX:0,aTy=0,aTZ=0,bTx=-0.03,bTy=-0.02,bTz:0.02,CTX=0,CTy=0,CTZ=0.在不斷調(diào)試過程中,發(fā)現(xiàn)姿態(tài)偏差補償比當(dāng)前姿態(tài)補償對系統(tǒng)誤差補償?shù)男Ч黠@,同時式中的常數(shù)項對靜態(tài)誤差有明顯的補償效果,一次項系數(shù)利二次項系數(shù)對動態(tài)誤差有明顯的補償效果.5試驗(Experiment)仿生眼系統(tǒng)軟硬件搭建完成后,分別對3個姿態(tài)角進行精度測試,姿態(tài)角通過LandMarkl0AHRS測量出來.通過上位機向控制器發(fā)送姿態(tài)角指令,控制器實時計算電機轉(zhuǎn)角并控制電機轉(zhuǎn)動相應(yīng)角度.同時為了驗證姿態(tài)閉環(huán)控制算法的有效性,分兩組進行對比測試,第一組是在開環(huán)控制情況下3個姿態(tài)角分別的精度測試,第二組是在姿態(tài)閉環(huán)控制情況下3個姿態(tài)角分別的精度測試,測試結(jié)果如8所示.根據(jù)圖8所示的3組對比實驗精度曲線,可以發(fā)現(xiàn)采用姿態(tài)閉環(huán)控制后,仿生眼姿態(tài)精度有了明顯的提高.在30.范圍內(nèi),精度從2.1.提高到了1.2.,精度從1.8.提高到了0.9.;-t-45.范圍內(nèi),精度從3.8.提高到了2.2.,精度從2.5.提高到了1.4.由于球面并聯(lián)機構(gòu)對稱的結(jié)構(gòu)特點,精度明顯優(yōu)于,且運動范圍大大超出設(shè)計范圍.40,?/20/旺0.一20:-40圖8姿態(tài)精度對比圖Fig.8Theimagesoftheattitudeaccuracy40C200蜱一20-40=加仃0D嘶m哪0o第33卷第3期李超等:基于姿態(tài)閉環(huán)控制的球面并聯(lián)仿生眼系統(tǒng)設(shè)計與研究3596結(jié)論(Conclusion)研究了基于姿態(tài)反饋的球面并聯(lián)機構(gòu)閉環(huán)控制方法,通過傳感器反饋的偏差值和姿態(tài)測量值對姿態(tài)進行補償,有效地提高了仿生眼的定位精度.制作了類似人眼運動特點,比人眼尺寸略大,具有控制信號和視頻信號接口的球面并聯(lián)仿生眼實物,基于DSP的控制系統(tǒng)實現(xiàn)了逆解和控制算法的實時在線計算.實驗結(jié)果表明,姿態(tài)精度得到有效提高,滿足正常視頻監(jiān)控和目標(biāo)跟蹤要求,系統(tǒng)T作穩(wěn)定.最后,本文提出的姿態(tài)閉環(huán)控制方法對其他并聯(lián)機構(gòu)位置閉環(huán)控制也有一定的借鑒作用.參考文獻(References)黃真,孔令富,方躍法.并聯(lián)機器人機構(gòu)學(xué)理論及控制M.北京:機械工業(yè)出版卞十,1997.HuangZ.KongLFFangYFMechanismtheoryandcontrolofparallelrobotM.Beijing:ChinaMachinePress,1997.閻洪祿,于秀敏.眼生理學(xué)M.北京:人民衛(wèi)生出版社,2001.YanHL,YuXM.PhysiologyoftheeyeM.Beijing:PeoplesMedicalPublishingHouse,2001.傘風(fēng)嗚.眼科全書M.北京:人民衛(wèi)生出版社,1996.LiFM.EncyclopaediaoftheeyeM.Beijing:PeoplesMedica/PublishingHouse,1996.CannataG,MaggiMiM.ImplementationofListingslawforatendondrivenroboteyeC/IEEE/RSJInternationalConferenceonIntelligentRobotsandSystems.Piscataway,NJ,USA:IEEE,2006:39403945.5黃田,曾憲菁,倪雁冰,等.球面并聯(lián)操作機的尺度綜合方法J.自然科學(xué)進展,2000,l0(8):751-756.HuangZengXJ,MiYB,eta1.Dimensionalsynthesisofsphericalparallelmanipulat0rs【JJ.ProgressinNaturalScience,2000,10(8):751-756.6】GosselinCM,HamelJETheagileeye:Ahighperformancethree?-degree?-of-freedomcamera-orientingdeviceC/IEEEIn?-ternationalConferenceonRoboticsandAutomation.Piscataway,NJ,USA:IEEE,1994:781786.【7GoseelinCM,StPierreE,GagneM.OnthedevelopmentoftheagileeyeJ.IEEERobotics&AutomationMagazine.1996.3(4):2937.8金振樸,曹舜,高峰.新型機器人肩關(guān)節(jié)及其運動學(xué)分析【J1.中國機械工程,2009,20(14):16391642.JinZL,CaoS,GaoEDesignandkinematicsanalysisofanovelrobotshoulderjointJ.ChinaMechanicalEngineering,2009,20(141:16391642.【9】劉和平,鄧力,江渝.數(shù)字信號處理器原理,結(jié)構(gòu)及應(yīng)用基礎(chǔ):TMS320F28xM1.北京:機械工業(yè)出版社,1997.LiuHPDengL.JiangYTheory,architectureandapplicationfundamentofdigitalsignalprocessor:TMS320F28xM.Beijing:ChinaMachinePress,2007.作者簡介:李超(1986一),男,碩士生.研究領(lǐng)域:機器人視覺與仿生H艮技術(shù).謝少榮(1972一),女,教授,博士生導(dǎo)師.研究領(lǐng)域:機器人視覺與仿生眼技術(shù).(上接第353頁)1012】13WisitsoraatA,PatthanasetakulVLomaseta1.LowcostthinfilmbasedpiezoresistiveMEMStactile