附录MATLAB 机器人工具箱仿真程序：1）运动学仿真模型程序（Rob1.m）L1=link([pi/2 150 0 0])L2=link([0 570 0 0])L3=link([pi/2 130 0 0])L4=link([-pi/2 0 0 640])L5=link([pi/2 0 0 0])L6=link([0 0 0 95])r=robot({L1 L2 L3 L4 L5 L6})=’MOTOMAN-UP6’ % 模型的名称>>drivebot(r)2）正运动学仿真程序（Rob2.m）L1=link([pi/2 150 0 0])L2=link([0 570 0 0])L3=link([pi/2 130 0 0])L4=link([-pi/2 0 0 640])L5=link([pi/2 0 0 0])L6=link([0 0 0 95])r=robot({L1 L2 L3 L4 L5 L6})=’MOTOMAN-UP6’t=[0:0.01:10];%产生时间向量qA=[0 0 0 0 0 0 ]; %机械手初始关节角度qAB=[-pi/2 -pi/3 0 pi/6 pi/3 pi/2 ];%机械手终止关节角度figure('Name','up6机器人正运动学仿真演示');%给仿真图像命名q=jtraj(qA,qAB,t);%生成关节运动轨迹T=fkine(r,q);%正向运动学仿真函数plot(r,q);%生成机器人的运动figure('Name','up6机器人末端位移图'）subplot(3,1,1);plot(t, squeeze(T(1,4,:)));xlabel('Time (s)');ylabel('X (m)');subplot(3,1,2);plot(t, squeeze(T(2,4,:)));xlabel('Time (s)');ylabel('Y (m)');subplot(3,1,3);plot(t, squeeze(T(3,4,:)));xlabel('Time (s)');ylabel('Z (m)');x=squeeze(T(1,4,:));y=squeeze(T(2,4,:));z=squeeze(T(3,4,:));figure('Name','up6机器人末端轨迹图'); plot3(x,y,z);3）机器人各关节转动角度仿真程序：（Rob3.m）L1=link([pi/2 150 0 0 ])L2=link([0 570 0 0])L3=link([pi/2 130 0 0])L4=link([-pi/2 0 0 640])L5=link([pi/2 0 0 0 ])L6=link([0 0 0 95])r=robot({L1 L2 L3 L4 L5 L6})='motoman-up6't=[0:0.01:10];qA=[0 0 0 0 0 0 ];qAB=[ pi/6 pi/6 pi/6 pi/6 pi/6 pi/6]; q=jtraj(qA,qAB,t);Plot(r,q);subplot(6,1,1);plot(t,q(:,1));title('转动关节1');xlabel('时间/s');ylabel('角度/rad');subplot(6,1,2);plot(t,q(:,2));title('转动关节2');xlabel('时间/s');ylabel('角度/rad');subplot(6,1,3);plot(t,q(:,3));title('转动关节3');xlabel('时间/s');ylabel('角度/rad');subplot(6,1,4);plot(t,q(:,4));title('转动关节4');xlabel('时间/s');ylabel('角度/rad' );subplot(6,1,5);plot(t,q(:,5));title('转动关节5');xlabel('时间/s');ylabel('角度/rad');subplot(6,1,6);plot(t,q(:,6));title('转动关节6');xlabel('时间/s');ylabel('角度/rad');4）机器人各关节转动角速度仿真程序：（Rob4.m）t=[0:0.01:10];qA=[0 0 0 0 0 0 ];%机械手初始关节量qAB=[ 1.5709 -0.8902 -0.0481 -0.5178 1.0645 -1.0201]; [q,qd,qdd]=jtraj(qA,qAB,t);Plot(r,q);subplot(6,1,1);plot(t,qd(:,1));title('转动关节1');xlabel('时间/s');ylabel('rad/s');subplot(6,1,2);plot(t,qd(:,2));title('转动关节2');xlabel('时间/s');ylabel('rad/s');subplot(6,1,3);plot(t,qd(:,3));title('转动关节3');xlabel('时间/s');ylabel('rad/s');subplot(6,1,4);plot(t,qd(:,4));title('转动关节4');xlabel('时间/s');ylabel('rad/s' );subplot(6,1,5);plot(t,qd(:,5));title('转动关节5');xlabel('时间/s');ylabel('rad/s');subplot(6,1,6);plot(t,qd(:,6));title('转动关节6');xlabel('时间/s');ylabel('rad/s');5）机器人各关节转动角加速度仿真程序：（Rob5.m）t=[0:0.01:10];%产生时间向量qA=[0 0 0 0 0 0]qAB =[1.5709 -0.8902 -0.0481 -0.5178 1.0645 -1.0201]; [q,qd,qdd]=jtraj(qA,qAB,t);figure('name','up6机器人机械手各关节加速度曲线');subplot(6,1,1);plot(t,qdd(:,1));title('关节1');xlabel('时间 (s)');ylabel('加速度 (rad/s^2)');subplot(6,1,2);plot(t,qdd(:,2));title('关节2');xlabel('时间 (s)');ylabel('加速度 (rad/s^2)');subplot(6,1,3);plot(t,qdd(:,3));title('关节3');xlabel('时间 (s)');ylabel('加速度 (rad/s^2)')subplot(6,1,4);plot(t,qdd(:,4));title('关节4');xlabel('时间 (s)');ylabel('加速度 (rad/s^2)')subplot(6,1,5);plot(t,qdd(:,5));title('关节5');xlabel('时间 (s)');ylabel('加速度 (rad/s^2)')subplot(6,1,6);plot(t,qdd(:,6));title('关节6');xlabel('时间 (s)');ylabel('加速度 (rad/s^2)')如有侵权请联系告知删除，感谢你们的配合！。
西安工程大学学报J o u r n a l o fX i 'a nP o l y t e c h n i cU n i v e r s i t y 第32卷第2期(总150期)2018年04月V o l .32,N o .2(S u m.N o .150) 文章编号:1674-649X (2018)02-0222-08D O I :10.13338/j.i s s n .1674-649x .2018.02.017 收稿日期:2017-12-18 基金项目:陕西省教育厅专项科研计划项目(16J K 1337) 通信作者:管声启(1971 ),男,西安工程大学副教授,博士,研究方向为机器零件质量检测㊁机器人视觉等.E -m a i l :s i n a 1300841@163.c o m 引文格式:王杰,管声启,曹帅.基于MA T L A B 的主操作手运动学分析及仿真[J ].西安工程大学学报,2018,32(2):222-229. WA N GJ i e ,G U A NS h e n g q i ,C A OS h u a i .K i n e m a t i c a l a n a l y s i s a n d s i m u l a t i o no fm a s t e rm a n i p u l a t o rb a s e do n MA T L A B [J ].J o u r n a l o fX i 'a nP o l y t e c h n i cU n i v e r s i t y,2018,32(2):222-229.基于MA T L A B 的主操作手运动学分析及仿真王 杰1,管声启1,曹 帅2(1.西安工程大学机电工程学院,陕西西安710048;2.西北工业大学机电学院,陕西西安710072)摘要:为实现对泌尿外科微创手术机器人的远程操作,给出一种用于位姿定位的主操作手结构.首先,根据手术任务确定主操作手的运动形式和自由度;其次,采用改进的D -H 法建立主操作手的运动学模型,分析其正㊁逆运动学和轨迹规划问题;最后,运用MA T L A B 软件对其进行运动学建模和仿真,得到主操作手的关节位移㊁关节速度㊁关节加速度随时间的变化曲线.结果验证了运动学模型的正确性和主操作手参数设计的合理性,为主操作手的进一步研制提供了理论基础.关键词:手术机器人;主操作手;轨迹规划;运动学中图分类号:T P241 文献标识码:A K i n e m a t i c a l a n a l ys i s a n d s i m u l a t i o no fm a s t e r m a n i pu l a t o r b a s e d o n M A T L A B WA N GJ i e 1,G U A N S h e n g qi 1,C A OS h u a i 2(1.S c h o o l o fM e c h a n i c a l a n dE l e c t r i c a l E n g i n e e r i n g ,X i 'a nP o l y t e c h n i cU n i v e r s i t y ,X i 'a n710048,C h i n a ;2.S c h o o l o fM e c h a n i c a l E n g i n e e r i n g ,N o r t h w e s t e r nP o l y t e c h n i c a lU n i v e r s i t y ,X i 'a n710072,C h i n a )A b s t r a c t :I no r d e r t o r e a l i z e t h e r e m o t e o p e r a t i o n o f am i n i m a l l y i n v a s i v e s u r g i c a l r o b o t f o r u r o l -o g y ,am a s t e rm a n i p u l a t o r s t r u c t u r e f o r p o s i t i o n p o s i t i o n i n g i s p r o p o s e d .F i r s t l y ,t h e s t r u c t u r a l c o n f i g u r a t i o n a n dD O Fo fm a s t e rm a n i p u l a t o r a r e c o n f i r m e d a c c o r d i n g t o t h e s u r g i c a l t a s k .S e c -o n d l y ,t h ek i n e m a t i c sm o d e l o f t h em a s t e rm a n i p u l a t o r i s e s t a b l i s h e dw i t ht h e i m p r o v e dD -H m e t h o da n d t h e p r o b l e m s o f f o r w a r dk i n e m a t i c s ,i n v e r s e k i n e m a t i c s a n d t r a j e c t o r yp l a n n i n g a r e a n a l y z e d .F i n a l l y ,t h ek i n e m a t i c sm o d e l i n g a n ds i m u l a t i o na r ec a r r i e do u tb y MA T L A Bs o f t -w a r e .T h e j o i n t d i s p l a c e m e n t ,j o i n t v e l o c i t y a n d j o i n t a c c e l e r a t i o nw i t h t i m e o fm a s t e rm a n i p u -l a t o r a r e o b t a i n e d ,w h i c hv e r i f y t h ec o r r e c t n e s so f t h ek i n e m a t i c sm o d e l a n dt h e r a t i o n a l i t y o ft h ed e s i g n p a r a m e t e r s f o rm a s t e rm a n i p u l a t o r .At h e o r e t i c a lb a s i s f o r f u r t h e rd e v e l o pm e n to f m a s t e rm a n i p u l a t o r i s p r o v i d e d .K e y w o r d s :s u r g i c a l r o b o t ;m a s t e rm a n i p u l a t o r ;t r a j e c t o r yp l a n n i n g ;k i n e m a t i c s 0 引 言近年来,微创手术成为外科手术的发展趋势,但在传统微创手术中,部分医生的触感较弱㊁精细操作比较困难,手部会不自觉地颤抖和疲劳[1].随着机器人技术在医疗领域的广泛应用,机器人辅助为传统微创手术带来了革命性的转变,不但有效克服了传统微创手术带来的弊端,而且极大拓展了医生的微创手术能力,能够显著缩短手术时间,提高手术质量[2].机器人微创手术凭借其创口小㊁恢复快㊁操作精确㊁医生不易疲劳等优势而逐渐成为外科手术的发展趋势[3-4].微创外科机器人可以自主地将手术器械按医生术前规划好的路径送达病灶位置,在一定程度上减少了手术创伤,消除了手部震颤,还能够降低手术医生的工作强度[5].而主操作手是主从式微创手术机器人的重要组成部分,是医生对手术机器人进行控制的直接 界面 ,在手术过程中,操作者要自始至终通过主操作手控制微创手术机器人及手术器械[6].主操作手参数设计是否合理,运动过程是否平稳,会直接影响其灵活度㊁回差㊁惯量㊁安全等性能.主操作手的运动学分析可为后续动力学分析提供可靠的理论依据,是实现主操作手运动控制和轨迹规划的基础,它主要包括正运动学和逆运动学两部分.主操作手的成本较高,对其进行实际操作和研究具有很大的局限性,通过仿真实验对其进行研究分析成为一种选择[7].文献[8-9]利用MA T L A B 软件对力反馈主操作手进行了实际工作空间和灵活度的仿真,并对力反馈模型进行了研究分析;文献[10-11]根据设计指标,对力反馈主操作手进行了建模与研究,并对其主要参数进行了分析与仿真;文献[12]主要对主操作手的设计效率和工作空间狭小问题进行了研究.总之,现阶段研究的用于远程遥控微创手术机器人的主操作手针对性较差,不能完全满足特定的手术需求,缺乏可操作性.本文针对如何实现泌尿外科微创手术机器人在良性前列腺增生(B P H ,b e n i g n p r o s t a t i ch y p e r p l a s i a )微创手术中的远程操作,根据具体手术需求,设计了专用于泌尿外科微创手术机器人系统的主操作手.以提出的专用于泌尿外科微创手术机器人的6自由度主操作手为例,采用改进的D -H 参数法研究分析其正㊁逆运动学问题,并借助MA T L A B 软件对其运动学和末端运动轨迹进行规划和仿真,以实现主操作手设计参数和运动过程满足临床手术对主操作手设计和运动的要求.1 主操作手总体设计1.1主操作手结构形式图1 主操作手结构示意图F i g .1D i a gr a mo f t h em a s t e r m a n i p u l a t o r 通过对手术任务要求的分析,泌尿外科微创手术机器人系统在启动后,需要手术器械进入到人体内寻找主操作手的姿态,使其姿态与主操作手姿态一致.但如果主操作手的姿态与器械的姿态相差太多,就会造成手术器械在人体内的动作幅度过大,很可能会划伤病灶周围的健康组织造成出血给病人带来额外痛苦.因此,正确合理的方式是:在泌尿外科微创手术机器人系统启动后,主操作手主动去寻找手术器械的姿态.而要实现这样的方式,就要求主操作手必须具备3个主动姿态自由度和3个主动位置自由度.在B P H 微创手术中,手术器械的主要运动可分为2种:一种是机器人从手运动确保手术器械定位,另一种是腕部关节旋转确保手术器械定向.具体来说,手术切入点允许手术器械有5个旋转自由度和1个直线自由度,来分别实现俯仰㊁偏航㊁绕自身轴线旋转和沿自身轴线的线性运动,以达到经尿道对前列腺增生部分进行电切除的目的.因此,针对上述目标,设计了一个可实现远程操作该手术机器人进行末端位姿定位的6自由度主操作手,如图1所示,其固连于全方位轮式底盘上,底盘的移动可进一步方便远程操作.322第2期 王杰,等:基于MA T L A B 的主操作手运动学分析及仿真图2 主操作手D -H 坐标系F i g .2D -Hc o o r d i n a t e s o f t h e m a s t e rm a n i p u l a t o r 1.2 主操作手位姿描述描述主操作手连杆的位置和姿态是对主操作手进行运动学分析的基础.由于连杆位姿问题可化归为坐标系位姿问题[13],因此,本文通过改进后的D -H [14]法建立主操作手各个关节的参考坐标系,如图2所示.z 为主操作手旋转关节的轴线或移动关节的移动方向沿线,x 为相邻关节轴线之间的公垂线.建立连杆坐标系后就可得出用于定量描述相对位姿的4种D -H 参数,如表1所示.其中a i -1和αi -1表示代表连杆自身结构信息的连杆长度和连杆扭角;θi 和d i 表示代表连杆连接关系信息的关节角和连杆平移量.因为提出的主操作手有5个旋转关节和一个移动关节,且旋转关节轴线都相互垂直,所以表中参数a i -1均为0,参数d i 除了d 3其余都为0.l 3为移动关节的初始伸长量,取l 3=20mm.根据坐标变换理论[14-15],齐次坐标变换矩阵i -1i T 可以描述相邻连杆坐标系{i -1}和{i }的相对位姿关系,反映相邻连杆的空间关系.i -1i T 为4ˑ4阶矩阵,对应由坐标系{i -1}到坐标系{i }的变换,其表达式为i -1i T =c o s θi -s i n θi 0a i -1c o s αi -1s i n θi c o s αi -1c o s θi -s i n αi -1-d i s i n αi -1s i n αi -1s i n θi s i n αi -1c o s θi c o s αi -1d i c o s αi -10001éëêêêêêùûúúúúú=i -1i R 3ˑ3i -1i P 3ˑ101ˑ31éëêêùûúú.(1)式中,i -1i R 3ˑ3为包含相邻连杆相对姿态信息的3ˑ3阶矩阵;i -1i P 3ˑ1为包含相邻连杆相对位置信息的3ˑ1阶矩阵.表1 操作机构D -H 参数表T a b l e 1 D -H p a r a m e t e r s o f t h e o p e r a t i n g me c h a n i s m i αi -1/(ʎ)a i -1/mm d i /mm θi /(ʎ)变量范围19000θ1+90-60ʎ~60ʎ29000θ2+90-60ʎ~60ʎ3-900d 3+i 30(0~230)mm 4000θ4-180ʎ~180ʎ5-9000θ5+90-90ʎ~90ʎ6-9000θ6-180ʎ~180ʎ2 主操作手运动学分析2.1 正运动学分析主操作手的运动学正问题是给定主操作手各连杆参数和关节变量,求解末端连杆坐标系相对于基坐标系的位姿.将表1中的各关节参数代入式(1),可得到相邻连杆之间的坐标转换矩阵01T ,12T ,23T ,34T ,45T ,56T .通过相邻连杆之间坐标转换矩阵,即可求得主操作手末端相对基坐标系的坐标转换矩阵,即该操作机构的末端姿态为06T =01T ㊃12T ㊃23T ㊃34T ㊃45T ㊃56T =n x o x a x p x n y o y a y p y n z o z a z p z 0001éëêêêêêùûúúúúú.(2)式(2)中n x =C 6(S 5(C 1S 4-C 4S 1S 2)-C 2C 5S 1)+S 6(C 1C 4+S 1S 2S 4),n y =C 6(C 5S 2-C 2C 4S 5)-C 2S 4S 6,n z =C 6(S 5(S 1S 4+C 1C 4S 2)+C 1C 2C 5)+S 6(S 1C 4-C 1S 2S 4).ìîíïïïï(3)422 西安工程大学学报 第32卷o x =C 6(C 1C 4+S 1S 2S 4)-S 6(S 5(C 1S 4-S 1S 2C 4)-S 1C 2C 5),o y =S 6(C 5S 2-C 2C 4S 5)-C 2S 4S 6,o z =C 6(S 1C 4+C 1S 4S 2)-S 6(S 5(S 1S 4-C 1S 2C 4)-C 1C 2C 5).ìîíïïïï(4)a x =C 5(C 1S 4+S 1S 2C 4)+S 1C 2S 5,a y =S 2S 5+C 2C 4C 5,a z =C 5(S 1S 4+C 1S 2C 4)-C 1C 2C 5.ìîíïïïï(5)p x =(d 3+l 3)S 1C 2,p y =(d 3+l 3)S 2,p z =-(d 3+l 3)C 1C 2.ìîíïïïï(6)式中,S i =s i n θi ,C i =c o s θi ,i ɪ[1,2,4,5,6].设主操作手末端在参考坐标系X 1,Y 1,Z 1中的位置P 1=[0 0 0 1]T ,在基坐标系x y z 中的位置为P =[P z P y P z 1]T ,则可以由式(7)确定末端P 点在基坐标系中的位置.从而得到主操作手末端的位置坐标,如式(8)所示.P =06T ㊃P 1.(7)P x P y P z 1éëêêêêêùûúúúúú=(d 3+l 2)S 1C 2(d 3+l 3)S 2-(d 3+l 3)C 1C 21éëêêêêêùûúúúúú.(8)由式(8)可知,主操作手末端位姿仅和前3个关节变量θ1,θ2,d 3有关,只要给定θ1,θ2,d 3的值,就可以求出主操作手末端在基坐标系中的位置,从而实现对主操作手有效的控制.2.2 逆运动学分析主操作手逆运动学是正运动学的逆过程,即已知主操作手末端的位姿矩阵,对主操作手各关节变量进行求解.由式(8)可知,主操作手末端的位置与腕部参数θ4,θ5,θ6无关,而只和前3个关节变量θ1,θ2,d 3有关,因此,可根据主操作手末端的参考点位置逆解出前3个位置关节变量的表达式,从而减小计算量[16].式(8)给出了主操作手在空间中的位置坐标,则通过对式(8)进行化简求解,即可求解出主操作手的位置变量θ1,θ2,d 3的值.为保证不丢失可能的解用双参量四象限反正切函数[17],对式(8)化简求解可得θ1=a r c t a n 2(x ,z ),(9)θ2=a r c s i n yx 2+y 2+z 2,(10)d 3=x 2+y 2+z 2-l 3.(11)图3 运动学仿真模型F i g .3T h e s i m u l a t i o nm o d e l o f k i n e m a t i c s 3 主操作手运动学仿真3.1 仿真模型的建立MA T L A B 软件具有强大的矩阵计算能力,运用其R o b o t i c t o o l b o x 工具箱,编制简单的算法程序,不仅可以实现对机器人的运动学可视化仿真,还可以快速对手工计算结果正确性进行验证[18].根据表1中的D -H 参数,运用R o b o t i c t o o l b o x 工具箱中的连杆函数指令L i n k 和r o b o t 函数对主操作手进行建模.建立的主操作手运动学仿真模型如图3所示.3.2 正运动学仿真验证为验证主操作手正运动学模型建立的正确性,利用R o b o t i c t o o l b o x 工具箱提供的f k i n e 函数进行实例计算,若计算结果与式(9)得出的结果完全相符合,则说明主操作手的正运动学模型建立正确.在关节变量的变化范围内给定N (N m a x =100)个关节变量和杆件参数,代入运动学方程中进行求解522第2期 王杰,等:基于MA T L A B 的主操作手运动学分析及仿真得到末端连杆坐标系相对基坐标系的设定位姿;再将对应的值输入主操作手的轨迹规划器中,直接读取实际的末端位姿.设定位姿和实际位姿的比较如图4所示,分析可知,设定值和实际值大体一致,说明所建立的正运动学模型准确可靠.图4 正运动学仿真结果F i g .4T h e s i m u l a t i o n r e s u l t s o f f o r w a r dk i n e m a t i c s 3.3 逆运动学仿真验证在常规的逆解验证过程中,由于R o b o t i c t o o l b o x 工具箱提供的逆解函数指令i k i n e 是运用一种数值迭代算法求解运动学逆问题,因此对任意的某一组具体位姿数值,存在由于迭代收敛性质引起的无解或漏解的错误情况,所以本文不采用i k i n e 函数指令全面验证主操作手的逆解,而是将用逆解实例计算结果得到的正运动学解和验证准确的正运动学解进行比较,若两者相比,误差在合理范围内,则逆解公式推导正确.为验证逆运动学方程式(9),式(10)和式(11)的正确性,在主操作手的可达工作范围内给定主操作手末端位姿,代入逆运动学方程中进行实例运算,从而可得主操作手的各关节变量.但需注意的是,在求解主操作手逆运动学时必须充分考虑逆解的存在性,即主操作手末端是否处于主操作手的可达工作空间内[19].因此,对应逆解的实例运算求解是否能顺利进行,取决于用于实例计算的目标点是否位于主操作手的可达工作空间内,且其对应的目标姿态是否能实现.任取5组处于主操作手可达工作空间内的位姿(表2)作为给定位姿进行逆解实例运算,可求得能实现给定位姿的逆解θ1,θ2,d 3,如表3所示.表2 主操作手可达工作空间内的位姿T a b l e 2 T h e p o s i t i o n t h a t t h em a s t e rm a n i p u l a t o r c a n r e a c h i n t h ew o r k s p a c e 组别p x p y p z 1-13.5105 6.1946-11.59392 2.6357-5.6267 -2.35783 21.9092-33.6649-14.60184 102.8947 4.3737-140.71635-108.1641-64.5030-109.1196表3 给定位姿对应的运动学逆解T a b l e 3 T h e i n v e r s ek i n e m a t i c s o f t h e p o s i t i o nc o r r e s p o nd i n g t o p o s i t i o n 组别θ1/(ʎ)θ2/(ʎ)d 3/mm 1-0.8616 0.334818.85012 0.8410-1.0097 6.64573 0.9829-0.907142.73814 0.6314 0.0251174.37755-0.7810-0.3975166.6349进一步的,在MA T L A B 中利用R o b o t i c t o o l b o x 工具箱提供的f k i n e 函数对表3的计算结果进行正运动学计算,可以到5组关节变量对应的位姿矩阵:T 1=-0.40410.78590.4681-13.5108-0.7768-0.0247-0.62926.1938-0.4829-0.61790.6205-11.59400001.0000éëêêêêêùûúúúúú,622 西安工程大学学报 第32卷T 2=-0.39150.6035-0.69472.6356-0.77960.18350.5988-5.62670.48880.77600.3986-2.35770001.0000éëêêêêêùûúúúúú,T 3=-0.38750.4723-0.791721.9078-0.83880.17560.5154-33.66570.38250.86370.3281-14.60190001.0000éëêêêêêùûúúúúú,T 4=0.29290.6781-0.6741102.8983-0.92920.0356-0.36794.3764-0.22540.73410.6405-140.71350001.0000éëêêêêêùûúúúúú,T 5=0.11380.80970.5757-108.1629-0.99300.11140.0397-64.5068-0.0319-0.57620.8167-109.11850001.0000éëêêêêêùûúúúúú.通过观察分析可知,5组正解位姿矩阵的最后一列元素和表2中的p x ,p y ,p z 相比较,误差在允许范围内,即用给定位姿的逆解所求得的位姿和给定位姿在理论上是一致的.因此,前述逆运动学分析所得的各关节变量计算公式是正确的.3.4 轨迹规划仿真验证轨迹规划是根据作业任务的要求来事先规定机器人的操作顺序和动作过程,轨迹规划仿真可以更详尽直观地描述机器人的运动过程[20].本文根据主控医生通过内窥镜和三维数字成像技术设计的机器人手术刀的理论运动轨迹,主要对主操作手在关节空间内进行从初始点到目标点运动过程中的轨迹规划,主要研究了主操作手和前3个关节之间的关系.取主操作手末端初始点A 的各关节变量为q A =[-0.5236 1.5708 50 -1.5708 -1.5708 0];目标点B 关节变量为q B =[0.7898 1.1733 166.6349 0.4488 -0.3927 0.5236].仿真时间为5s ,在MA T L A B 中编程并运行,可分别得到主操作手仿真末端轨迹图㊁位移图以及旋转关节J 1,J 2和移动关节J 3的位移变化曲线㊁速度变化曲线㊁加速度变化曲线,如图5~7所示.由仿真图可知,主操作手末端在初始点A 到目标点B 运动过程中连续平稳;旋转关节J 1,J 2,移动关节J 3,位移变化持续稳定;关节J 1,J 2,J 3的速度㊁加速度初末值均为零,且曲线连续光滑,没有突变,说明主操作手在运动过程中比较平稳,整个结构不会产生较大振动.从而说明主操作手连杆参数设计合理,准确可靠. 图5 主操作手仿真末端轨迹图 图6 主操作手仿真末端位移图F i g .5T h e e n d t r a j e c t o r y o f s i m u l a t i o n F i g .6T h e e n dd i s pl a c e m e n t s i m u l a t i o no f m a s t e rm a n i p u l a t o r m a s t e rm a n i p u l a t o r 722第2期 王杰,等:基于MA T L A B 的主操作手运动学分析及仿真图7 主操作手关节J 1,J 2,J 3的位移㊁速度和加速度变化曲线F i g .7T h e c h a n g e c u r v e o f d i s p l a c e m e n t ,v e l o c i t y a n da c c e l e r a t i o no f J 1,J 2a n d J 3f o rm a s t e rm a n i p u l a t o r 4 结束语针对如何实现泌尿外科微创手术机器人的远程操作,设计一种用于位姿定位的主操作手结构,并结合MA T L A B 软件对其进行分析与验证.首先,根据目标任务,设计了具有6个自由度的主操作手结构;其次,采用改进的D -H 法和坐标变换理论求解主操作手的正运动学问题;在此基础上求解主操作手运动学逆解;最后,运用MA T L A B 软件中的R o b o t i c t o o l b o x 工具箱对主操作手进行运动学建模和仿真,得到主操作手的末端轨迹㊁末端位移以及主要关节的位移㊁速度㊁加速度随时间的变化曲线.结果表明,主操作手参数设计合理,运动学模型建立准确可靠,末端轨迹符合预期要求,为主操作手后期的动力学分析㊁结构优化和运动控制提供了必要依据.参考文献(R e f e r e n c e s ):[1] S H E N T ,N E L S O NCA ,WA R B U R T O NK ,e t a l .D e s i g n a n d a n a l ys i s o f a n o v e l a r t i c u l a t e d d r i v em e c h a n i s mf o rm u l t i -f u n c t i o n a lN O T E S r o b o t [J ].J o u r n a l o fM e c h a n i s m s&R o b o t i c s ,2015,7(1):110041-110048.[2] 王伟,王伟东,董为,等.基于协作空间与灵巧度的机器人辅助微创手术术前规划算法[J ].机器人,2016,38(2):208-216. WA N G W ,WA N G W D ,D O N G W ,e t a l .A p r e o p e r a t i v e p l a n n i n g a l g o r i t h mb a s e do nd e x t e r i t y a n d c o l l a b o r a t i o n s pa c e f o r t h e r ob o t -a s s i s t e dm i n i m a l l y i n v a s i v e s u r g e r y [J ].R o b o t ,2016,38(2):208-216.[3] 牛孝霞.微创手术机器人主手结构设计与分析[D ].哈尔滨:哈尔滨工业大学,2014:9-12.N I UXX.S t r u c t u r e d e s i n g n a n d a n a l y s i s o fm a n i p u l a t o r f o rm i n i m a l l y i n v a s i v e r o b o t i c s u r g e r y s ys t e m [D ].H a r b i n :H a r -b i n I n s t i t u t e o fT e c h n o l o g y ,2014:9-12.[4] 沈桐,宋成利,徐兆红.新型混联腹腔镜手术机器人的运动学建模与优化[J ].机械科学与技术,2016,35(1):56-62.S H E N T ,S O N GCL ,X UZ H.K i n e m a t i c sm o d e l i n g a n do p t i m i z a t i o no f ah y b r i dr o b o t i cm a n i p u l a t o r f o r l a p a r o s c o pi c s u r g e r y [J ].M e c h a n i c a l S c i e n c e a n dT e c h n o l o g y f o rA e r o s p a c eE n g i n e e r i n g ,2016,35(1):56-62.[5] 倪自强,王田苗,刘达.医疗机器人技术发展综述[J ].机械工程学报,2015,51(13):45-52.N I ZQ ,WA N G T M ,L I U D.S u r v e y o nm e d i c a l r o b o t i c s [J ].J o u r n a l o fM e c h a n i c a l E n g i n e e r i n g,2015,51(13):45-52.[6] 屈金星,冯丰.力反馈型主操作手工作空间分析与仿真方法研究[J ].机械传动,2016(7):53-57. Q UJX ,F E N GF .S t u d y o n t h e a n a l y z i n g a n d s i m u l a t i n g m e t h o do fw o r k s p a c e o f f o r c e r e f l e c t i n g m a i nm a n i pu l a t o r [J ].822 西安工程大学学报 第32卷J o u r n a l o fM e c h a n i c a lT r a n s m i s s i o n ,2016(7):53-57.[7] 韦攀东,李鹏飞,王晓华,等.抓取柔软织物多指灵巧手的建模与仿真[J ].西安工程大学学报,2016,30(3):300-305.W E I PD ,L I PF ,WA N GX H ,e t a l .M o d e l i n g a n d s i m u l a t i o n o fm u l t -i f i n g e r e d d e x t e r o u s h a n d o f f l e x i b l e f a b r i c g r a s p-i n g [J ].J o u r n a l o fX 'i a nP o l y t e c h n i cU n i v e r s i t y ,2016,30(3):300-305.[8] 王宏民.遥操作骨科复位手术的力反馈主操作手及时延控制研究[D ].哈尔滨:哈尔滨工业大学,2015.WA N G H M.R e s e a r c h o n t h e f o r c e f e e d b a c km a s t e rm a n i p u l a t o r a n d t i m e d e l a y c o n t r o l o f t e l e o p e r a t i o n o r t h o pe d i c s u r -g e r y s y s t e m [D ].H a r b i n :H a r b i n I n s t i t u t e o fT e c h n o l o g y ,2015.[9] 姜运祥.力反馈主手设计及主从操作技术研究[D ].秦皇岛:燕山大学,2014:32-40.J I A N GYX.R e m o t e f o r c e f e e d b a c km a s t e r h a n dd e s i g n a n dm a s t e r -s l a v e o p e r a t i o n [D ].Q i n h u a n gd a o :Y a n s h a nU n i ve r -s i t y ,2014:32-40[10] 白杨.新型力反馈手控器研究[D ].北京:北京邮电大学,2014:25-27.B A IY.R e s e a r c ho n t h en o v e l f o r c e f e e d b a c kh a p t i c d e v i c e [D ].B e i j i n g :B e i j i n g U n i v e r s i t y ofP o s t s a n dT e l e c o mm u n i -c a t i o n s ,2014:25-27.[11] Y U Y ,G U OJ ,G U OS ,e t a l .M o d e l l i n g a n da n a l y s i so f t h ed a m p i n g f o r c e f o r t h em a s t e rm a n i pu l a t o ro f t h e r o b o t i c c a t h e t e r s y s t e m [C ]//I E E EI n t e r n a t i o n a l C o n f e r e n c e o n M e c h a t r o n i c s a n dA u t o m a t i o n .B e i j i n g :I E E E ,2015:693-697.[12] L E E H ,C H E O NB ,HWA N G M ,e t a l .A m a s t e rm a n i p u l a t o rw i t ha r e m o t e -c e n t e r -o f -m o t i o nk i n e m a t i c s t r u c t u r e f o r am i n i m a l l y i n v a s i v e r o b o t i c s u r g i c a l s y s t e m [J ].T h e I n t e r n a t i o n a l J o u r n a l o fM e d i c a l R o b o t i c s a n dC o m p u t e rA s s i s t e d S u r g e r y ,2018,14(1):1865-1877.[13] 黄旭楠,岳夏,冼颂智.基于MA T L A B 的码垛机械手运动学分析与仿真[J ].机床与液压,2017,45(5):35-39.HU A N G X N ,Y U EX ,X I A NSZ .K i n e m a t i c s a n a l y s i s a n d s i m u l a t i o no f p a l l e t i z i n g m a n i pu l a t o rb a s e do n MA T L A B [J ].M a c h i n eT o o l&H y d r a u l i c s ,2017,45(5):35-39.[14] 蔡自兴.机器人学基础[M ].北京:机械工业出版社,2013:83-88.C A I ZX.F u n d a m e n t a l s o f r o b o t i c s [M ].B e i j i n g :C h i n aM a c h i n eP r e s s ,2013:83-88.[15] C H E N X ,X I N X ,Z HA OB ,e t a l .D e s i g na n da n a l y s i so f ah a p t i cm a s t e rm a n i p u l a t o r f o rm i n i m a l l y i n v a s i v e s u r g e r y[C ]//I E E EI n t e r n a t i o n a l C o n f e r e n c e o n I n f o r m a t i o na n dA u t o m a t i o n .S i n g a p o r e :I E E E ,2017:260-265.[16] G U OJ ,Y U Y ,G U O S ,e t a l .D e s i g na n d p e r f o r m a n c ee v a l u a t i o no f an o v e lm a s t e rm a n i pu l a t o r f o r t h er o b o t -a s s i s t c a t h e t e r s y s t e m [C ]//I E E EI n t e r n a t i o n a l C o n f e r e n c e o n M e c h a t r o n i c s a n dA u t o m a t i o n .H a r b i n :I E E E ,2016:937-942.[17] 董辉,李林鑫.水平多关节机械手运动学分析与仿真[J ].浙江工业大学学报,2013,41(5):562-566.D O N G H ,L ILX.K i n e m a t i c s a n a l y s i s a n d s i m u l a t i o n o f t h e h o r i z o n t a lm u l t i -j o i n tm a n i p u l a t o r [J ].J o u r n a l o f Z h e j i a n gU n i v e r s i t y o fT e c h n o l o g y ,2013,41(5):562-566.[18] HO N GJ ,J I A N G Y ,P E N G S ,e ta l .D e s i g no fan o v e l f o r c e f e e d b a c k m a s t e rm a n i p u l a t o r [J ].I F A C -P a pe r s O n L i n e ,2015,48(28):1444-1449.[19] 赵建文,宋胜涛,李瑞琴,等.2-R P R+R R P 球面混联机构的可达工作空间研究[J ].机械传动,2017(1):24-27.Z HA OJW ,S O N GST ,L IR Q ,e t a l .R e s e a r c ho f r e a c h a b l ew o r k s p a c e o f 2-R P R+R R Ps p h e r i c a l h yb r i dm ec h a n i s m [J ].J o u r n a l o fM e c h a n i c a lT r a n s m i s s i o n ,2017(1):24-27.[20] 乐英,岳艳波.六自由度机器人运动学仿真及轨迹规划[J ].组合机床与自动化加工技术,2016(4):89-92.L EY ,Y U EYB .T h ek i n e m a t i c s s i m u l a t i o na n d t r a j e c t o r yp l a n n i n g of s i x -D O Fr o b o t [J ].M o d u l a rM a c h i n eT o o l&A u t o m a t i cM a n u f a c t u r i ng T e ch ni q u e ,2016(4):89-92.责任编辑:孟 超922第2期 王杰,等:基于MA T L A B 的主操作手运动学分析及仿真。
