基于姿态闭环控制的球面并联仿生眼系统设计与研究.doc

基于姿态闭环控制的球面并联仿生眼系统设计与研究第33卷第3期2011年5月机器人ROBOT)1.33.No.3May,2011DoI:1O.3724/SEJ.1218.2011.00354基于姿态闭环控制的球面并联仿生眼系统设计与研究李超,谢少荣,李恒宇,缪金松,徐元玉,罗均(上海_人学机电工程与自动化学院,二海200072)摘要:提出了一种基于姿态反馈的球面并联机构闭环控制方法,有效地解决了该机构输入与输出之间复杂的3维非线性强耦合映射关系给构建闭环系统带来的问题.问时将球面并联机构用到机器人眼设计上,制作了类似人跟运动特点,比人眼尺寸略大,具有控制和视频信号接口的仿生眼实物:基于数字信号处理器(DSP)的控制系统实现了逆解和闭环控制算法的实时在线计算.实验结果表明,姿态闭环控制算法有效提高了仿生眼的定位精度.关键词:球面并联机构;机器人眼;仿生眼;闭环控制,中图分类号: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)球面并联机构是一种可实现三自由度转动的l并联机构I1j,这种运动特点正好与人眼的运动特点(.?相同.参考人眼运动特点和结构特点设计的球面并联仿生眼,弥补了当前绝大部分机器人眼只能实现二自由度转动而缺少绕视轴自旋的缺憾,降低了机器人自身姿态改变和目标本体姿态变化对接收视频图像质量的影响,减轻了人眼观察和计算机处理这些视频信号的难度.在仿生眼三维模型的运动仿真结果与理论计算结果完全一致后,构建了基于DSP的控制系统对仿生眼进行运动控制.但在此过程中,我们发现由于各零件尺寸很小,结构复杂且空间尺寸多等冈素影响,零件的加工精度和装配精度等很难满足设计要求,仿生眼有一定的定位误差,尤其是在大角度定位时.为了解决该问题,本文提出一种基于姿态反馈的球面并联机构闭环控制方法,通过在眼球上安装姿态传感器反馈实时姿态,有效地解决了球面并联机构输入与输出之间复杂的3维非线性强耦合映射关系给构建闭环系统带来的诸多问题,提高了仿生眼的定位精度.2眼球运动特点及Fick坐标系(Movingcharacteristicsofeyeandcoordinatesys.temsofFick)眼球运动在视觉中起着至关重要的作用,它可以近似地看作是眼球绕其几何中心的3自由度球面运动I3】.它由6块眼外肌控制,眼外肌的伸缩能使眼球向各个方向转动,这些伸缩是在大脑调节中枢控制之下完成的.每眼各有6条眼外肌,如图1所基金项目:国家863计划资助项目(2007AA04Z225,2009AA04Z211);国家自然科学基金资助项目(60975068)通讯作者:李超,1收稿/录用/修同:20100913/2010.11-03/2011-0223第33卷第3期李超等:基于姿态闭环控制的球面并联仿生眼系统设计与研究355示.在眼外肌的驱动下,通常眼球上下和左转动角度可达5:38.,白旋角可达15.通过眼球旋转中心设有3个假想的轴,即Fick三轴,此3轴相互垂直构成Fick坐标系2_水平轴(1,轴),通过两眼旋转中心的假想轴,围绕此轴眼球能作上下转动;垂直轴(x轴),通过眼球旋转中心的一根从上向并与水平轴垂直的假想轴,围绕此轴眼球能作左右转动;前后轴(z轴),通过眼球旋转中心并与水平轴和垂直轴成正交的一根从前向后的假想轴,同绕此轴眼球能作内外侧旋;Listing平面,假想的通过眼球旋转中心和水平轴以及垂轴的平面4J.Z1眼外肌和Fick坐标系Fig.1ExtraocularmuscleandcordinatesystemsofFickZ3球面并联仿生眼设计及姿态逆解(DesignofbioniceyeofSPMandattitudecalcula.tionofinversekinematics)3.1球面并联仿生眼运动特点球面并联机构结构简图如图2所示,它由上平台,底座和3对由下连杆与上连杆构成的支链组成.同连在底库上的电机与下连杆,下连杆与上连杆,上平台与上连杆均由转动副连接,9个转动副轴线相交丁0点,该点称为机构的转动中心,过D点并与底座平行的平面称为球心平面1,5-7.在3个电机的联合驱动卜,上平台相对底座可实现3白由度旋转.该运动特点恰好与眼球运动特点相同,且它们的外形利驱动形式都十分相似,故将球面并联机构府用在仿生眼设计上.机构的球心平面看作是眼球的Listing平面,以球心D为原点建立的固定坐标系D一看作是眼球的Fick坐标系.球面并联机构共有5个结构参数:,】,77,其中,分别为下连杆和上连杆的结构夹角;卢,卢2分别为底座和上平台的半锥角;叼为初始状态时上平台和底座的结构扭角.参考人眼的结构特点优选结构参数(=60.,a2=60.,1=3O.,=130.,77=127.)进行设计,根据该参数设计的仿生眼在外形和运动范罔上都与人眼相近,图3为根据这些参数制作的仿生眼实物,其眼球半径22.5mm,最大截面半径50mm.图2球面并联机构结构图Fig.2ThestructuresofSPM图3仿生眼实物Fig.3BioniceyeofSPM3.2仿生眼姿态逆解如图2,以球心D为原点建立定坐标系D一,动坐标系O-XYZ和连杆坐标系0一ff(,J=1,2,3).Zo轴为底座中心H与原点D的连线,正方向指向上平台;z轴为原点D与上平台中心的连线,正方向指向上平台;,轴为第个分支的第.,个转动副方向,正方向指向球外;Xo轴方向为机器人2011年5月轴和zl1轴所成平面的法线方向;x轴方向为Z0轴和z13轴所成平面的法线方向;,轴方向为由与川轴所成平面的法线方向;各系j,轴方向由右手法则确定.眼球相对于底座只有转动,眼球姿态可由动坐标系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个姿态角.利用向量与向量w的夹角为,建立约束方程4-81:,Mf=0.5,f=1,2,3(5)整理:Afsin0/+BiCOSOi+C/=0(6)解方程,得到:=2arctan(7)或=2arctan22(8)式中Af,Bf,G由,求出,根据图3的装配形式,式(8)为仿生眼的姿态逆解,为了简化表达,用下式表示:=,R(,)(9)4控制系统设计(Designofcontrolsystem)4.1硬件设计求解仿生眼姿态逆解过程繁琐,需要大量的三角函数和反三角函数等复杂函数计算,且逆解需要在线实时计算,这对控制器的运算速度和运算精(4)度要求很高.根据这些要求,控制器选用TMS320F2812型号DSP,它特别适用于有大量数据处理的控制场合,运行时钟达150MHz,且引导ROM保存了标准的数学函数表,可以方便地进行各种函数的调用_9J.姿态传感器选用美国GladiatorTechnologies公司的LandMarkl0AHRS全硅6自由度数字姿态和航向参考系统(AHRS).它提供RS485输出,包括deltavelocity,deltatheta,水平转角,俯仰角和自旋角.角度静态精度可达0.2.,动态精度0.5.,采样频率可达200Hz,这些性能满足系统需要.为保证仿生眼能够稳定高效地工作,采用图4所示的结构进行系统硬件设计,并利用LabVlEW编写上位机(图5)对眼球姿态进行控制,对整个系统进行监控和数据分析.4.2姿态闭环控制算法研究在实际控制过程中,由于零件加工精度和装配精度等因素影响,仿生眼的定位精度较差,尤其在入角度转动时(火于等于30.).为提高系统精度,在眼球上安装LandMarklOAHRS(图6),利用它对眼球姿态的实时测量与反馈,对姿态进行闭环控制.由于仿生眼3个姿态角和3个电机转角之间是一种复杂的3维非线性强耦合映射关系,每个电机转角都由3个姿态角共同确定,因此传统的单轴位置闭环控制算法在仿生眼的姿态闭环控制上已不适用,更不能简单地根据姿态误差直接对眼球姿态进第33卷第3期李超等:基于姿态闭环控制的球商并联仿生眼系统设计与研究357一一一,/一一一一一一,图4硬件体系Fig.4Hardwarearchitectures图5监摔界面Fig.5Monitorinterface图6仿生眼和姿态传感器Fig.6Bioniceyeandattitudetransducer行补偿.要通过姿态误差对眼球姿态进行准确的补偿,必须对姿态误差产生的原冈进行分析.该系统中,姿态输入值与姿态实际值之间的姿态误差是由各零件加l精度,装配精度,电机定位精度,传感器精度以及随机误差等冈素共同决定,其中前3个冈素可以归结为结构参数的实际值与设计值之间的偏着,这里的结构参数是0f,卢,这5个参数在3个支链中各自的实际值.受加L:精度等冈素影响,它们并不是精确地对称相等.而式(9)的姿态逆解模型,是建立在各尺寸都对称相等,且都与设计值相同的理想情况下,因此式(9)是对实际姿态逆解的近似表示,需要尽可能利用其他信息对式(9)的近似表示进行完善,使系统能够更准确地进行定位.该系统中,可利用的信息有姿态误差和姿态实测值.将式(9)的逆解模型作为算法的核心,通过姿态误差和姿态实测值对姿态进行补偿,建立7所示的姿态闭环控制算法./.,I虽I=fR()=(+-厂M()+(T)(10)/式中,=ll为3个姿态角输入值,I量=l00203l为3个电机转角输出值,=y为姿态测量值,=为姿态偏差,=+十为姿态校正后输入值,.为逆解模型,厂M为姿态偏差值补偿函数,.为姿态实测值补偿函数.图7姿态闭环控制原理图Fig.7Schematicofattitudeclosedloopcontrol358机器人2011年5月姿态偏差值补偿函数.由于系统非线性强耦合特征,单方向偏差补偿跟3个方向姿态误差以及其他因素都有密切关系,因此很难总结出一个准确的偏差补偿函数.为简化系统设计难度,假设单方向偏差补偿函数只由该方向的误差决定,采用次项方程近似表示两者关系:axayaz+bx000by000bzCX000Cy000CZ()()().+式中,ax,ay,az为常数,bbbz为一次项系数,CX,Ccz为二次项系数.姿态实测值补偿函数,N.参考l厂M,同样假设单方向姿态实测值补偿函数只由该方向姿态实测值决定,采用二次项方程近似表示两者关系:(痧yrZ+bTx000bTy000bTz00CTy00CTZ婊加x衍yz+(12)式中,nTy,为常数,bTbTy,bTz为一次项4020O_204O系数,CTX,cTCTz为二次项系数.在调试过程中,根据实际控制效果,对式(1o)和(11)中各常数和系数不断地进行调整,以达到最佳的定位精度,系统采用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.在不断调试过程中,发现姿态偏差补偿比当前姿态补偿对系统误差补偿的效果更明显,同时式中的常数项对静态误差有明显的补偿效果,一次项系数利二次项系数对动态误差有明显的补偿效果.5试验(Experiment)仿生眼系统软硬件搭建完成后,分别对3个姿态角进行精度测试,姿态角通过LandMarkl0AHRS测量出来.通过上位机向控制器发送姿态角指令,控制器实时计算电机转角并控制电机转动相应角度.同时为了验证姿态闭环控制算法的有效性,分两组进行对比测试,第一组是在开环控制情况下3个姿态角分别的精度测试,第二组是在姿态闭环控制情况下3个姿态角分别的精度测试,测试结果如8所示.根据图8所示的3组对比实验精度曲线,可以发现采用姿态闭环控制后,仿生眼姿态精度有了明显的提高.在30.范围内,精度从2.1.提高到了1.2.,精度从1.8.提高到了0.9.;-t-45.范围内,精度从3.8.提高到了2.2.,精度从2.5.提高到了1.4.由于球面并联机构对称的结构特点,精度明显优于,且运动范围大大超出设计范围.40,?/20/旺0.一20:-40图8姿态精度对比图Fig.8Theimagesoftheattitudeaccuracy40C200蜱一20-40=加仃0D嘶m哪0o第33卷第3期李超等:基于姿态闭环控制的球面并联仿生眼系统设计与研究3596结论(Conclusion)研究了基于姿态反馈的球面并联机构闭环控制方法,通过传感器反馈的偏差值和姿态测量值对姿态进行补偿,有效地提高了仿生眼的定位精度.制作了类似人眼运动特点,比人眼尺寸略大,具有控制信号和视频信号接口的球面并联仿生眼实物,基于DSP的控制系统实现了逆解和控制算法的实时在线计算.实验结果表明,姿态精度得到有效提高,满足正常视频监控和目标跟踪要求,系统T作稳定.最后,本文提出的姿态闭环控制方法对其他并联机构位置闭环控制也有一定的借鉴作用.参考文献(References)黄真,孔令富,方跃法.并联机器人机构学理论及控制M.北京:机械工业出版卞十,1997.HuangZ.KongLFFangYFMechanismtheoryandcontrolofparallelrobotM.Beijing:ChinaMachinePress,1997.阎洪禄,于秀敏.眼生理学M.北京:人民卫生出版社,2001.YanHL,YuXM.PhysiologyoftheeyeM.Beijing:PeoplesMedicalPublishingHouse,2001.伞风呜.眼科全书M.北京:人民卫生出版社,1996.LiFM.EncyclopaediaoftheeyeM.Beijing:PeoplesMedica/PublishingHouse,1996.CannataG,MaggiMiM.ImplementationofListingslawforatendondrivenroboteyeC/IEEE/RSJInternationalConferenceonIntelligentRobotsandSystems.Piscataway,NJ,USA:IEEE,2006:39403945.5黄田,曾宪菁,倪雁冰,等.球面并联操作机的尺度综合方法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金振朴,曹舜,高峰.新型机器人肩关节及其运动学分析【J1.中国机械工程,2009,20(14):16391642.JinZL,CaoS,GaoEDesignandkinematicsanalysisofanovelrobotshoulderjointJ.ChinaMechanicalEngineering,2009,20(141:16391642.【9】刘和平,邓力,江渝.数字信号处理器原理,结构及应用基础:TMS320F28xM1.北京:机械工业出版社,1997.LiuHPDengL.JiangYTheory,architectureandapplicationfundamentofdigitalsignalprocessor:TMS320F28xM.Beijing:ChinaMachinePress,2007.作者简介:李超(1986一),男,硕士生.研究领域:机器人视觉与仿生H艮技术.谢少荣(1972一),女,教授,博士生导师.研究领域:机器人视觉与仿生眼技术.(上接第353页)1012】13WisitsoraatA,PatthanasetakulVLomaseta1.LowcostthinfilmbasedpiezoresistiveMEMStactile