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

基于姿態(tài)閉環(huán)控制的球面并聯(lián)仿生眼系統(tǒng)設計與研究第33卷第3期2011年5月機器人ROBOT)1.33.No.3May,2011DoI:1O.3724/SEJ.1218.2011.00354基于姿態(tài)閉環(huán)控制的球面并聯(lián)仿生眼系統(tǒng)設計與研究李超,謝少榮,李恒宇,繆金松,徐元玉,羅均(上海_人學機電工程與自動化學院,二海200072)摘要:提出了一種基于姿態(tài)反饋的球面并聯(lián)機構閉環(huán)控制方法,有效地解決了該機構輸入與輸出之間復雜的3維非線性強耦合映射關系給構建閉環(huán)系統(tǒng)帶來的問題.問時將球面并聯(lián)機構用到機器人眼設計上,制作了類似人跟運動特點,比人眼尺寸略大,具有控制和視頻信號接口的仿生眼實物:基于數(shù)字信號處理器(DSP)的控制系統(tǒng)實現(xiàn)了逆解和閉環(huán)控制算法的實時在線計算.實驗結果表明,姿態(tài)閉環(huán)控制算法有效提高了仿生眼的定位精度.關鍵詞:球面并聯(lián)機構;機器人眼;仿生眼;閉環(huán)控制,中圖分類號:TP273文獻標識碼: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)機構是一種可實現(xiàn)三自由度轉動的l并聯(lián)機構I1j,這種運動特點正好與人眼的運動特點(.?相同.參考人眼運動特點和結構特點設計的球面并聯(lián)仿生眼,彌補了當前絕大部分機器人眼只能實現(xiàn)二自由度轉動而缺少繞視軸自旋的缺憾,降低了機器人自身姿態(tài)改變和目標本體姿態(tài)變化對接收視頻圖像質量的影響,減輕了人眼觀察和計算機處理這些視頻信號的難度.在仿生眼三維模型的運動仿真結果與理論計算結果完全一致后,構建了基于DSP的控制系統(tǒng)對仿生眼進行運動控制.但在此過程中,我們發(fā)現(xiàn)由于各零件尺寸很小,結構復雜且空間尺寸多等岡素影響,零件的加工精度和裝配精度等很難滿足設計要求,仿生眼有一定的定位誤差,尤其是在大角度定位時.為了解決該問題,本文提出一種基于姿態(tài)反饋的球面并聯(lián)機構閉環(huán)控制方法,通過在眼球上安裝姿態(tài)傳感器反饋實時姿態(tài),有效地解決了球面并聯(lián)機構輸入與輸出之間復雜的3維非線性強耦合映射關系給構建閉環(huán)系統(tǒng)帶來的諸多問題,提高了仿生眼的定位精度.2眼球運動特點及Fick坐標系(Movingcharacteristicsofeyeandcoordinatesys.temsofFick)眼球運動在視覺中起著至關重要的作用,它可以近似地看作是眼球繞其幾何中心的3自由度球面運動I3】.它由6塊眼外肌控制,眼外肌的伸縮能使眼球向各個方向轉動,這些伸縮是在大腦調節(jié)中樞控制之下完成的.每眼各有6條眼外肌,如圖1所基金項目:國家863計劃資助項目(2007AA04Z225,2009AA04Z211);國家自然科學基金資助項目(60975068)通訊作者:李超,1收稿/錄用/修同:20100913/2010.11-03/2011-0223第33卷第3期李超等:基于姿態(tài)閉環(huán)控制的球面并聯(lián)仿生眼系統(tǒng)設計與研究355示.在眼外肌的驅動下,通常眼球上下和左轉動角度可達5:38.,白旋角可達15.通過眼球旋轉中心設有3個假想的軸,即Fick三軸,此3軸相互垂直構成Fick坐標系2_水平軸(1,軸),通過兩眼旋轉中心的假想軸,圍繞此軸眼球能作上下轉動;垂直軸(x軸),通過眼球旋轉中心的一根從上向并與水平軸垂直的假想軸,圍繞此軸眼球能作左右轉動;前后軸(z軸),通過眼球旋轉中心并與水平軸和垂直軸成正交的一根從前向后的假想軸,同繞此軸眼球能作內外側旋;Listing平面,假想的通過眼球旋轉中心和水平軸以及垂軸的平面4J.Z1眼外肌和Fick坐標系Fig.1ExtraocularmuscleandcordinatesystemsofFickZ3球面并聯(lián)仿生眼設計及姿態(tài)逆解(DesignofbioniceyeofSPMandattitudecalcula.tionofinversekinematics)3.1球面并聯(lián)仿生眼運動特點球面并聯(lián)機構結構簡圖如圖2所示,它由上平臺,底座和3對由下連桿與上連桿構成的支鏈組成.同連在底庫上的電機與下連桿,下連桿與上連桿,上平臺與上連桿均由轉動副連接,9個轉動副軸線相交丁0點,該點稱為機構的轉動中心,過D點并與底座平行的平面稱為球心平面1,5-7.在3個電機的聯(lián)合驅動卜,上平臺相對底座可實現(xiàn)3白由度旋轉.該運動特點恰好與眼球運動特點相同,且它們的外形利驅動形式都十分相似,故將球面并聯(lián)機構府用在仿生眼設計上.機構的球心平面看作是眼球的Listing平面,以球心D為原點建立的固定坐標系D一看作是眼球的Fick坐標系.球面并聯(lián)機構共有5個結構參數(shù):,】,77,其中,分別為下連桿和上連桿的結構夾角;盧,盧2分別為底座和上平臺的半錐角;叼為初始狀態(tài)時上平臺和底座的結構扭角.參考人眼的結構特點優(yōu)選結構參數(shù)(=60.,a2=60.,1=3O.,=130.,77=127.)進行設計,根據(jù)該參數(shù)設計的仿生眼在外形和運動范罔上都與人眼相近,圖3為根據(jù)這些參數(shù)制作的仿生眼實物,其眼球半徑22.5mm,最大截面半徑50mm.圖2球面并聯(lián)機構結構圖Fig.2ThestructuresofSPM圖3仿生眼實物Fig.3BioniceyeofSPM3.2仿生眼姿態(tài)逆解如圖2,以球心D為原點建立定坐標系D一,動坐標系O-XYZ和連桿坐標系0一ff(,J=1,2,3).Zo軸為底座中心H與原點D的連線,正方向指向上平臺;z軸為原點D與上平臺中心的連線,正方向指向上平臺;,軸為第個分支的第.,個轉動副方向,正方向指向球外;Xo軸方向為機器人2011年5月軸和zl1軸所成平面的法線方向;x軸方向為Z0軸和z13軸所成平面的法線方向;,軸方向為由與川軸所成平面的法線方向;各系j,軸方向由右手法則確定.眼球相對于底座只有轉動,眼球姿態(tài)可由動坐標系O-XYZ相對同定坐標系D一Zn的方向余弦矩陣Rxyz來表示5-81.Jyz=c加cssc一cscs妒zssCrs+cc一ssccss妒ZccIccl式中,加,分別表示眼球的左右轉角,俯仰角,自旋角,s,c分別表示sin,COS.wi定義向量:|沿】方向,1,f沿Z2方向,w,沿3方向.通過何姿變換求出向量蜥,在固定坐標系D一z=0卜的方向矢量.一一c+vFJss3ccc一c一44(3)式中叩1l=0.,rh2=120.,rh3=240.,(i=1,2,3)為3個電機的轉角.同樣通過位姿變換可以求出向量在動坐標系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)設計(Designofcontrolsystem)4.1硬件設計求解仿生眼姿態(tài)逆解過程繁瑣,需要大量的三角函數(shù)和反三角函數(shù)等復雜函數(shù)計算,且逆解需要在線實時計算,這對控制器的運算速度和運算精(4)度要求很高.根據(jù)這些要求,控制器選用TMS320F2812型號DSP,它特別適用于有大量數(shù)據(jù)處理的控制場合,運行時鐘達150MHz,且引導ROM保存了標準的數(shù)學函數(shù)表,可以方便地進行各種函數(shù)的調用_9J.姿態(tài)傳感器選用美國GladiatorTechnologies公司的LandMarkl0AHRS全硅6自由度數(shù)字姿態(tài)和航向參考系統(tǒng)(AHRS).它提供RS485輸出,包括deltavelocity,deltatheta,水平轉角,俯仰角和自旋角.角度靜態(tài)精度可達0.2.,動態(tài)精度0.5.,采樣頻率可達200Hz,這些性能滿足系統(tǒng)需要.為保證仿生眼能夠穩(wěn)定高效地工作,采用圖4所示的結構進行系統(tǒng)硬件設計,并利用LabVlEW編寫上位機(圖5)對眼球姿態(tài)進行控制,對整個系統(tǒng)進行監(jiān)控和數(shù)據(jù)分析.4.2姿態(tài)閉環(huán)控制算法研究在實際控制過程中,由于零件加工精度和裝配精度等因素影響,仿生眼的定位精度較差,尤其在入角度轉動時(火于等于30.).為提高系統(tǒng)精度,在眼球上安裝LandMarklOAHRS(圖6),利用它對眼球姿態(tài)的實時測量與反饋,對姿態(tài)進行閉環(huán)控制.由于仿生眼3個姿態(tài)角和3個電機轉角之間是一種復雜的3維非線性強耦合映射關系,每個電機轉角都由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)設計與研究357一一一,/一一一一一一,圖4硬件體系Fig.4Hardwarearchitectures圖5監(jiān)摔界面Fig.5Monitorinterface圖6仿生眼和姿態(tài)傳感器Fig.6Bioniceyeandattitudetransducer行補償.要通過姿態(tài)誤差對眼球姿態(tài)進行準確的補償,必須對姿態(tài)誤差產生的原岡進行分析.該系統(tǒng)中,姿態(tài)輸入值與姿態(tài)實際值之間的姿態(tài)誤差是由各零件加l精度,裝配精度,電機定位精度,傳感器精度以及隨機誤差等岡素共同決定,其中前3個岡素可以歸結為結構參數(shù)的實際值與設計值之間的偏著,這里的結構參數(shù)是0f,盧,這5個參數(shù)在3個支鏈中各自的實際值.受加L:精度等岡素影響,它們并不是精確地對稱相等.而式(9)的姿態(tài)逆解模型,是建立在各尺寸都對稱相等,且都與設計值相同的理想情況下,因此式(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個電機轉角輸出值,=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)誤差以及其他因素都有密切關系,因此很難總結出一個準確的偏差補償函數(shù).為簡化系統(tǒng)設計難度,假設單方向偏差補償函數(shù)只由該方向的誤差決定,采用次項方程近似表示兩者關系:axayaz+bx000by000bzCX000Cy000CZ()()().+式中,ax,ay,az為常數(shù),bbbz為一次項系數(shù),CX,Ccz為二次項系數(shù).姿態(tài)實測值補償函數(shù),N.參考l廠M,同樣假設單方向姿態(tài)實測值補償函數(shù)只由該方向姿態(tài)實測值決定,采用二次項方程近似表示兩者關系:(痧yrZ+bTx000bTy000bTz00CTy00CTZ婊加x衍yz+(12)式中,nTy,為常數(shù),bTbTy,bTz為一次項4020O_204O系數(shù),CTX,cTCTz為二次項系數(shù).在調試過程中,根據(jù)實際控制效果,對式(1o)和(11)中各常數(shù)和系數(shù)不斷地進行調整,以達到最佳的定位精度,系統(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.在不斷調試過程中,發(fā)現(xiàn)姿態(tài)偏差補償比當前姿態(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)角指令,控制器實時計算電機轉角并控制電機轉動相應角度.同時為了驗證姿態(tài)閉環(huán)控制算法的有效性,分兩組進行對比測試,第一組是在開環(huán)控制情況下3個姿態(tài)角分別的精度測試,第二組是在姿態(tài)閉環(huán)控制情況下3個姿態(tài)角分別的精度測試,測試結果如8所示.根據(jù)圖8所示的3組對比實驗精度曲線,可以發(fā)現(xiàn)采用姿態(tài)閉環(huán)控制后,仿生眼姿態(tài)精度有了明顯的提高.在30.范圍內,精度從2.1.提高到了1.2.,精度從1.8.提高到了0.9.;-t-45.范圍內,精度從3.8.提高到了2.2.,精度從2.5.提高到了1.4.由于球面并聯(lián)機構對稱的結構特點,精度明顯優(yōu)于,且運動范圍大大超出設計范圍.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)設計與研究3596結論(Conclusion)研究了基于姿態(tài)反饋的球面并聯(lián)機構閉環(huán)控制方法,通過傳感器反饋的偏差值和姿態(tài)測量值對姿態(tài)進行補償,有效地提高了仿生眼的定位精度.制作了類似人眼運動特點,比人眼尺寸略大,具有控制信號和視頻信號接口的球面并聯(lián)仿生眼實物,基于DSP的控制系統(tǒng)實現(xiàn)了逆解和控制算法的實時在線計算.實驗結果表明,姿態(tài)精度得到有效提高,滿足正常視頻監(jiān)控和目標跟蹤要求,系統(tǒng)T作穩(wěn)定.最后,本文提出的姿態(tài)閉環(huán)控制方法對其他并聯(lián)機構位置閉環(huán)控制也有一定的借鑒作用.參考文獻(References)黃真,孔令富,方躍法.并聯(lián)機器人機構學理論及控制M.北京:機械工業(yè)出版卞十,1997.HuangZ.KongLFFangYFMechanismtheoryandcontrolofparallelrobotM.Beijing:ChinaMachinePress,1997.閻洪祿,于秀敏.眼生理學M.北京:人民衛(wèi)生出版社,2001.YanHL,YuXM.PhysiologyoftheeyeM.Beijing:PeoplesMedicalPublishingHouse,2001.傘風嗚.眼科全書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.自然科學進展,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金振樸,曹舜,高峰.新型機器人肩關節(jié)及其運動學分析【J1.中國機械工程,2009,20(14):16391642.JinZL,CaoS,GaoEDesignandkinematicsanalysisofanovelrobotshoulderjointJ.ChinaMechanicalEngineering,2009,20(141:16391642.【9】劉和平,鄧力,江渝.數(shù)字信號處理器原理,結構及應用基礎:TMS320F28xM1.北京:機械工業(yè)出版社,1997.LiuHPDengL.JiangYTheory,architectureandapplicationfundamentofdigitalsignalprocessor:TMS320F28xM.Beijing:ChinaMachinePress,2007.作者簡介:李超(1986一),男,碩士生.研究領域:機器人視覺與仿生H艮技術.謝少榮(1972一),女,教授,博士生導師.研究領域:機器人視覺與仿生眼技術.(上接第353頁)1012】13WisitsoraatA,PatthanasetakulVLomaseta1.LowcostthinfilmbasedpiezoresistiveMEMStactile