毕业设计（论文）-基于手脚融合的多足步行机器人的运动精度研究 44页

基于手脚融合的多足步行机器人的运动精度研究河南工程学院本科毕业设计（论文）题目基于手脚融合的多足步行机器人的运动精度研究学生姓名专业班级机设06-4班学号号院（系）机电工程学院指导教师（教授）完成时间2010年5月30日III\n基于手脚融合的多足步行机器人的运动精度研究基于手脚融合的多足步行机器人的运动精度研究摘要机器人运动误差是衡量机器人性能的重要指标之一，直接影响到机器人的工作质量，对多足步行机器人的运动精度研究是一项重要而富有意义的工作。本文以四足步行机器人为研究对象，通过分析机器人的正运动学和逆运动学运动方程和误差方程，对步行机器人的精度分析问题进行了比较深入的理论研究。首先，结合国内外多足步行机器人现状和误差分析现状以及多足步行机器人的机构特点，分析了运动误差的主要来源。其次，在多足步行机器人正逆运动学的求解过程的基础上，运用微分的方法计算运动机器人正逆运动的误差方程。最后，在MATLAB环境下编制相应的误差分析程序，通过实例仿真验证误差分析的正确性。关键词：多足步行机器人/正运动学/逆运动学/误差分析III\n基于手脚融合的多足步行机器人的运动精度研究HAND-FOOT-INTEGRATEDMECHANUSMTHEMOREWALKINGROBOTFUSIONACCURACYOFMOVEMENTABSTRACTThemotionerrorisoneoftheimportantindexesrobotperformances,whichaffecttheworkingqualityofthemultileggedwalkingrobotdirectly,formuchofthemotionprecisionwalkingrobotresearchisanimportantandmeaningfulwork.Basedonfourwalkingrobotforresearchobject,throughtheanalysisoftherobotkinematicsandinversekinematicsisinerrorequationsequationandthewalkingrobot,precisionanalysisproblem,deeplytheoreticalresearch.First,combinedwiththedomesticandforeignmanywalkingrobotpresentsituationandtheerroranalysis,aswellasthecharacteristicsofthewalkingrobot,theerroranalysisofmovementerroristhemainreasonforthedifference.Secondly,inwalkingrobotistheinversekinematicsolutionprocess,usingthedifferentialmethodofinverserobotmotionequationofmovementerror.Finally,intheMATLABenvironmentcorrespondingerroranalysisprogramcompiledbyexampletoprovethecorrectnessoftheerroranalysis.Keywords:multi-leggedwalkingrobot，forwardkinematics，inversekinematics，ErroranalysisIII\n基于手脚融合的多足步行机器人的运动精度研究目录摘要IABSTRACTII1绪论11.1引言11.2课题来源、目的及意义11.3国内外研究现状21.4本文研究内容72多足步行机器人的正运动学分析和误差分析92.1引言92.2研究对象的介绍102.3影响运动误差的主要因素112.4机器人正运动学分析和误差分析132.4.1串联机械手的正运动学132.4.2机器人抓取时的正运动学分析162.4.3算例192.5本章小结213多足步行机器人的逆运动学分析和误差分析223.1机器人的逆运动学分析和误差分析223.2算例263.3本章小结28结论29致谢30参考文献31附录33附录133附录237III\n基于手脚融合的多足步行机器人的运动精度研究1绪论1.1引言在自然界和人类社会中,存在一些人类无法到达的地方和可能危及人类生命的特殊场合,如行星表面、灾难发生矿井、防灾救援和反恐斗争等,对这些危险环境进行不断地探索和研究,寻求一条解决问题的可行途径成为科学技术发展和人类社会进步的需要。具有仿生特征的移动机器人，因为能够代替人在一些非结构性环境中作业而成为了学者们研究和关注的热点。传统的移动机器人主要包括履带式、足式、轮式、混合式等多种运动形式。其中，履带式和轮式机器人结构较简单，其运动能力受到环境因素的限制。相对于轮式、履带式机器人而言，多足步行机器人在非结构化、存在不确定性的环境内移动虽具有较大的优势，但现有的多足步行机器人通常作为一种单纯的移动平台,或者配置特定的机械臂才能完成作业。如果能够在腿/臂融合结构基础上，把多足机器人的腿设计成具有手脚融合功能的结构形式，可使其能在在更多特殊环境和场合中使用,因而该类机器人具有广阔的应用前景。鉴于此，为了拓展多足步行机器人的实际应用,在世界范围内的科研人员对多足步行机器人的结构及所配工具开展了广泛的研究，如日本东京工业大学开发了TITAN-IX型排雷四足步行机器人。瞄准国内外机器人技术的前沿，为了给我国步行机器人的研究提供理论平台和关键技术，开展多足机器人的技术和相关理论研究具有重要的科学意义和应用价值。1.2课题来源、目的及意义课题来源于国家自然科学基金（编号：）：本文是分析多足步行机器人运动时误差产生的原因，建立误差分析的模型，对误差参数经行分析。步行机器人(walkingrobot)或步行车辆(walkingvehicle)40\n基于手脚融合的多足步行机器人的运动精度研究简称步行机,是一种智能型机器人,它是涉及到生物科学、仿生学、机构学、传感技术及信息处理技术等的一门综合性高科技。在崎岖路面上,步行机器人优于轮式或履带式车辆。步行机器人腿式系统具有很大的优越性：较好的机动性,崎岖路面上乘坐的舒适性,对地形的适应能力强。所以,这类机器人在军事运输、海底探测、矿山开采、星球探测、残疾人的轮椅、教育及娱乐等众多行业,有非常广阔的应用前景,多足步行机器人技术一直是国内外机器人领域的研究热点之一。定位精度是衡量多足机器人性能的一个重要指标，因此，无论在理论上还是在实验当中都受到了国内外学者的广泛关注。目前，由于工业机器人的广泛应用，针对其误差的研究已经受到了广大学者的关注，而多足步行机器人尚未能像工业机器人那样大规模的应用,其基础理论的研究比较滞后,关于其位姿误差分析的研究自今很少涉及。通过多足步行机器人的位姿误差分析，可以得到各个误差源对机器人机构输出位姿的影响程度，从而可以发现机构中的关键环节，明确提高机器人精度的重点和方向，为改善机器人的设计质量和提高机器人的设计水平提供准确可靠的资料和依据。因此，在多足步行机器人领域对精度进行研究是一项重要而富有实际意义的工作。本课题的研究将介绍一种多足步行机器人的误差分析的方法，以四足步行机器人为例，通过仿真验证该方法的可行性。该课题的研究将会促进多足步行机器人向实用化迈进。1.1国内外研究现状1.3.1多足步行机器人的研究现状多足步行机器人的发展最早可追溯到中国古代三国时的“木牛流马”。有据可查的是在1893年Rygg设计的机械马，历经一个多世纪的发展，特别是随着20世纪后期计算机技术、电子技术、人工智能技术、生物工程的飞速发展，多足步行机器人的研究已经取得了长足的进步。20世纪60年代初，由美国的Shigley和Baldwin设计出了比履带车或轮式车更为灵活的步行机。比较典型的是由Mosher（美国）在1968年设计的“WalkingTruck”四足车[1]，如图1-1所示。其四条腿采用液压驱动，手臂和脚安装有位置传感器，具有步行和爬越障碍的功能，因此，“WalkingTruck”被视为步行机发展史上一个里程碑。在1976年，日本的ShiegoHirose成功研制了世界上第一台四足步行机器人KUMO，如图1-2所示。它的外形像一个蜘蛛，有四条腿，能够爬行[2]。1983年由美国研制的“ODEX-I”六足步行机器人，6条腿沿圆周均布,且每条腿有3个自由度,适于在狭小空间里运动,还可以上下台阶。1984-1986年东京大学的Shimoyama和Miura研制了Collie-l四足机器人，如图l-3所示。在1986-1988又研制了Collie-2[3]，如图l-4所示。Collie-2每条腿有5个关节，且每个关节都装有电位器。该机器人装有实时操作系统，实现了trot和pace步态。1984-1986年东京大学的Shimoyama和Miura研制了Collie-l四足机器人，如图l-340\n基于手脚融合的多足步行机器人的运动精度研究图11步行机“WalkingTruck”图12第一台四足步行机器人KUMO所示。在1986-1988又研制了Collie-2[3]，如图l-4所示。Collie-2每条腿有5个关节，且每个关节都装有电位器。该机器人装有实时操作系统，实现了trot和pace步态。图13Colliel图14Collie-2图15“Spider-robot”图16“Attila”美国NASA研制的微型爬行机器人“Spider-bot”，如图1-5所示,机器人外形象蜘蛛,40\n基于手脚融合的多足步行机器人的运动精度研究重量轻,体积只有人头部的一半大小,可以在不规则的星球表面爬行。1990年初由美国MITAILab完成的仿昆虫有腿行走机器人Attila,如图1-6所示。Attila采用了模块化设计,头、腿、身体都有各自的驱动器、传感器和子处理器，有19个自由度。S.Hirose等研制的TITAN系列四足步行机器人历经了八代。TITAN-III[4]，其足由形状记忆合金组成，且装有信号处理系统和传感器，可以自动检测与地面接触的状态。1994年研制了TITAN-VII，其能够躲避障碍和在陡峭和崎岖的地方步行。1996年研制的四足机器人TITAN-VIII，如图1-7所示，它具有很高的地面适应能力，腿能够作为工作臂，用于排雷和探测地雷[5]。图17TITAN-VIII图18Scout-I加拿大McGill大学研制了四足机器人Scout-I[6]，如图1-8所示，机器人只有四个自由度，每条腿有一个转动关节。能转弯、步行和跨台阶，但可靠性较差。Scout-II[7]，如图1-9所示，能完成奔跑和步行等运动。图19Scout-II图110BISAM1995年，日本的安達、小谷内等研究了手脚统一型步行机器人MELMANTIS[8],能将脚的移动和手臂的操作统一起来。该机器人可进行地雷探测、森林采伐和拆除作业等。1998年由德国开发的四足机器人BISAM[9]40\n基于手脚融合的多足步行机器人的运动精度研究，如图1-10所示。该机器人4条腿完全相同，每条腿之间由3个转动关节相连，另一个转动关节连接躯干和腿部。该机器人实现了实时控制。西班牙开发的四足机器人SIL04，如图1-11所示。机器人每条腿有3个回转关节，并装有倾角器、编码器、力传感器和电位器。能在不平地面上行走，并能躲避障碍物。图111SIL04图112ASTERISK2005年由大阪大学的新井健生、田窪明仁等研制成功的新型手脚统一型步行机器人ASTERISK[8]，如图1-12所示。该机器人有6条腿,且每条腿有4个自由度，具有用手搬运物品及用脚移动或进行作业的双重机能，具有全方向移动的机能和全方位均等的作业空间，可悬吊于天花板进行作业或在不平地面上移动。采用电机驱动，有6个CCD摄像机和11个传感器。国内从20世纪80年代末90年代初开始研究步行机[8]。近年来,对多足步行机器人相关技术的研究取得了一系列成果。1980年,中国科学院研制成功了八足步行机器人。1989年,北京航空航天大学研究成功了四足步行机器人。1990年,中国科学院沈阳自动化研究所研制出了六足步行机器人[10]。同年,清华大学也研制成功了全方位三足步行机器人DTWN。1991年,上海交通大学研制了四足步行机器人JTUWM系列[11]。2000年,上海交通大学研制了微型双三足步行机器人MDTWR。2002年,上海交通大学研制了微型六足仿生机器人[12]。华中科技大学研制的4+2多足步行机器人[13]。从目前国内外多足步行机器人的研究现状可看出，多足步行机器人多作为一种移动平台，很难实现复杂的操作功能。对有手脚融合功能的多足步行机器人的研究极少涉及。2005年由大阪大学的新井健生、田窪明仁等研制成功的新型手脚统一型步行机器人ASTERISK[8]，如图1-12所示。该机器人有6条腿,40\n基于手脚融合的多足步行机器人的运动精度研究且每条腿有4个自由度，具有用手搬运物品及用脚移动或进行作业的双重机能，具有全方向移动的机能和全方位均等的作业空间，可悬吊于天花板进行作业或在不平地面上移动。采用电机驱动，有6个CCD摄像机和11个传感器。国内从20世纪80年代末90年代初开始研究步行机[8]。近年来,对多足步行机器人相关技术的研究取得了一系列成果。1980年,中国科学院研制成功了八足步行机器人。1989年,北京航空航天大学研究成功了四足步行机器人。1990年,中国科学院沈阳自动化研究所研制出了六足步行机器人。同年,清华大学也研制成功了全方位三足步行机器人DTWN。1991年,上海交通大学研制了四足步行机器人JTUWM系列。2000年,上海交通大学研制了微型双三足步行机器人MDTWR。2002年,上海交通大学研制了微型六足仿生机器人。华中科技大学研制的4+2多足步行机器人[14]。从目前国内外多足步行机器人的研究现状可看出，多足步行机器人多作为一种移动平台，很难实现复杂的操作功能。对有手脚融合功能的多足步行机器人的研究极少涉及。1.3.2行机器人误差研究的现状机器人的实到位姿与理论位姿之间的偏差，称为机器人的位姿误差，这个指标直接影响到多足步行机器人定位精度。在很多应用场合，多足步行机器人机身可作为作业平台搭载仪器设备，因此其定位精度直接影响到机器人的工作质量。目前，有很多学者对并联机器人误差进行了研究，由于多足步行机器人在瞬时类似于具有冗余驱动的并联机构，因此，对并联机器人的误差分析理论也可用于多足步行机器人的误差研究中。在国外，已有不少学者对机器人误差建模进行了研究。早在1978年，KJWaldron和AKuman就提出了操作机器人的位置误差问题。次年，他们又对机器人位置精度提出了机器人位姿误差建模的矩阵法，在D-H坐标系中，采用两个3×3旋转变换矩阵和一个3维平移列矢量作为相邻构件之间的转换矩阵，并假设结构参数已知，且不存在任何误差，建立了机器人末端执行器的位置误差表达式。后来，AKuman和SPrakash引入结构参数误差，导出了综合考虑运动变量误差和结构参数误差的机器人末端执行器的位置误差表达式。Chi-haurWu将机器人机构运动速度分析方法应用到静态误差分析中来，导出了由于构件结构参数误差和关节运动变量误差引起的末端执行器位置误差变化规律。WangSM和Ehmann利用坐标转换方法，针对并联机构驱动器误差、铰链自身误差及铰链定位误差建立可以直接微分的输入输出方程然后进行直接微分，进行误差建模。Timo提出一种分析机器人精度的方法，根据输入输出方程微分推导求得输出误差与驱动误差与尺寸误差的关系方程。HanSKim对Stewart平台的并联机器人运动误差范围作了分析与综合,40\n基于手脚融合的多足步行机器人的运动精度研究通过对误差模型特征值问题的分析，研究了特征值与位姿误差和铰点误差界限的关系，提出了Stewart平台误差界限正逆解问题的分析方法，并以双三角Stewart平台为仿真对象，分析了动平台平移和转动时特征值的变化情况。在国内，关于并联机器人的误差理论已成为学者的研究热点[15-20]。黄真教授[21]采用螺旋理论对并联机器人位姿误差进行了分析，其研究内容是分析已知尺寸误差，控制误差以及运动副间隙对末端位姿误差的影响。汪劲松教授[22]将Stewart平台的各分支作为假想的单开链，利用串联机器人运动学D-H方法，结合从运动学方程微分得到的结论，推导出Stewart平台并联机器人终端运动误差和铰链间隙误差间的映射关系。徐卫良[23]则通过直接对各个原始误差的微小位移矢量进行合成，建立了机器人手部位姿误差的数学模型。利用蒙特卡洛技术模拟服从某种概率分布的原始误差，抽样计算机器人手部位姿误差，然后在数值上完成了机器人在其可达工作空间内的位姿误差的各种概率分析。在误差概率分析的基础上，建立了以连杆参数公差为设计变量、公差制造成本为目标函数、绝对位姿误差满足设计精度为约束条件的机器人机构精度优化设计的数学模型。洪林[24]从典型结构Stewart平台出发，运用并联机器人输入输出微分关系，建立了机器人输出位姿误差正解的数学模型，全面分析了结构参数和位姿参数对输出位姿误差的影响问题，研究了不同参数下奇异位形和机器人位姿正负偏差最大值出现的位置，结合对位姿正负偏差最大值的控制，提出了结构参数合理取值以有效避开误差敏感区的可行方法。焦国太[25]在刚性机器人精度分析的基础上，结合柔性机器人的研究成果，对机器人位姿误差的分析，充分考虑了连杆和关节柔性以及静态误差对机器人末端执行器位姿精度的影响，将刚性机器人的精度分析方法和柔性机器人的运动学和动力学研究相结合，建立了机器人的位姿误差分析模型。祃琳[36]以3－HSS型并联机床为对象，利用空间矢量链模型建立零部件制造误差与动平台位姿误差的映射关系，当给定零部件制造公差后，利用该模型即可预估出动平台的位姿误差。在此基础上，根据灵敏度分析结果，提出一种在给定刀具位姿允差条件下，以零部件制造公差加权欧氏范数最大为目标，以角性公差和线性公差在同一精度等级下达到均衡为约束条件的精度设计方法。1.1本文研究内容本文以四足步行机器人为研究对象，在讨论其运动学的基础上，提出了误差建模与分析的方法，并结合实验验证该方法的可行性.本文的研究对于提高多足步行机器人的定位精度，运动稳定性和优化多足步行机器人的设计具有重要意义。40\n基于手脚融合的多足步行机器人的运动精度研究根据机器人的理论模型设计一条直线步态，通过对机器人在该步态下实际运动中机体轨迹误差数据的测量，通过机器人的误差模型，计算出站立腿驱动关节参数误差值。将该误差值的负值作为驱动关节参数的补偿量，按补偿后的关节参数重新驱动多足步行机器人运动该步态实验结果表明：机器人机体轨迹的运动精度显著提高。本文主要结构如下：(1)绪论。(2)简述步行机器人发展现状及机器人误差分析的研究现状。(3)多足步行机器人的运动学介绍。计算手脚融合状态下的正运动学和逆运动学的误差，建立机器人误差分析模型。(4)利用Matlab开发机器人的运动误差分析程序。(5)将以前述的误差分析为基础，对由多种因素引起的机器人的综合位姿误差进行探讨，并对实例进行实验,验证了误差分析方法的正确性40\n基于手脚融合的多足步行机器人的运动精度研究1多足步行机器人的正运动学分析和误差分析1.1引言多足步行机器人的运动学计算是求解机器人的输入变量与输出构件（机身平台）之间的位置关系，是机器人运动分析最基本的任务，也是机器人速度、加速度、受力分析、工作空间分析、误差分析、动力分析等的基础。在多足步行机器人运动过程中，各条腿按顺序抬起和放下，从瞬时性角度来看，多足步行机器人就是一个具有冗余自由度的并联机构。由于多足步行机器人结构复杂，对其进行运动学计算要比一般的移动机器人的运动学计算要复杂的多。已知机器人主动件的位置，求解机器人输出件的位置和姿态称为正运动学求解，若已知输出件的位置和姿态，求解机器人输入件的位置称逆运动学求解。类似于并联机器人，多足步行机器人的正运动求解比逆运动学求解要复杂得多。本文将对正运动学和逆运动学分别求解。并联机器人运动学分析主要有数值法和解析法。数值法的优点是它可以应用于任何结构的并联机器人，计算方法简单，但不能保证获得全部解，存在局部极小点问题，计算时间较长。本章将以四足步行机器人为研究对象介绍一种解析法求解多足步行机器人正运动学的方法。图2.1步行机器人MiniQuad的关节布置示意图40\n基于手脚融合的多足步行机器人的运动精度研究1.1研究对象的介绍MiniQuad是由郑州轻工业学院开发的四足机器人样机,它是一种具有腿臂融合、模块化特征的小型多足步行机器人。MiniQuad由机体和四条腿组成。机器人机体是一个矩形平台。每条腿通过一个旋转关节与矩形平台相连。为了保证机器人的机动性，旋转关节的轴线与矩形平台垂直。图2.1给出了MiniQuad的一般简图。其腿类似于同一机器人机体上的四个机械手。每条腿由一个平面连杆机构、一个旋转机构和一只脚组成。通常，除了一些自由或被动关节以外，每条腿还有三个执行关节，其中代表旋转关节，而和代表平面连杆机构中的关节。如图2.1所示,每条腿上的活动关节通常都由直流电机驱动。由于机器人的重量都集中在其机体上，旋转机构的关节支撑连接着腿和机体，所以可以根据几何对称性假设机器人的重心与其几何中心近似一致。当一条腿与地面接触时，该腿定义为站立腿，并假设该接触点是不可动的。如果腿处于摆动状态，该腿就被定义为摆动腿。将腿与地面通过运动学耦合，从而该机构系统就相当于一个并联机械手。对于多足机器人静态稳定的步行运动，最基本的步行模式是三条腿同时支撑于地面上，其余的腿向前摆动。机器人步行时它的运动机构可以看作一个是由机器人机体（运动平台）、地面（固定平台）和三条站立腿构成的并联机械手和摆动腿组合而成。图2.2给出了其中一条腿处于摆动状态时四足机器人的结构简图。图2.2四足机器人瞬时步行状态其中代表参考坐标系，在这里我们选定自然坐标系作为参考坐标系，代表固定在机器人机体上、原点与机器人几何中心重合的坐标系，机器人矩形机体的两边尺寸为40\n基于手脚融合的多足步行机器人的运动精度研究和。1.1影响运动误差的主要因素多足步行机器人的运动学D-H参数法坐标变换中坐标变换矩阵及平台位姿变换矩阵都是不考虑各运动学参数误差的理想变换，但在实际应用中，无论机器人制造精度多高，都会由于各种原因引起机器人运动学参数误差，影响运动平台定位精度。机器人的位姿精度取决于很多因素，其因素主要包括机器人零部件的加工制造误差，机器人的安装，传动机构的误差，机器人连杆和关节的柔性及机器人工作环境等因素。影响多足步行机器人位姿精度的因素可分为静态因素和动态因素。所谓静态因素是在机器人运动过程中始终保持不变的因素，所谓动态因素是指在机器人运动过程中随时间变化的因素。静态因素包括：1.多足步行机器人连杆结构参数和运动变量误差。由于制造及装配的误差使机器人腿关节的实际参数与其名义值之间产生偏差,从而引起机器人位姿误差；2.环境因素。如工作环境温度的变化将导致机器人连杆的长度尺寸误差；3.关节误差、位置传感器误差、控制系统的误差等；4.环境变化所引起的立足点位置误差。动态因素主要包括：由自重、外力、惯性力等引起的腿部连杆和关节的弹性变形及振动。其中，对多足步行机器人定位精度影响最大的误差因素包括：（1）机器人的连杆参数误差多足步行机器人腿部各关节都是旋转关节，对于旋转关节有3个结构参数,即D-H参数,和,其误差主要是由于机器人在制造和安装过程中产生的，或则体现为步行机器人连杆（腿）的长度尺寸。机器人腿部连杆长度尺寸误差可导致机器人平台的位姿误差。通常引起连杆长度误差（）的因素主要有连杆的加工误差及因周围环境温度变化而引起的连杆长度尺寸误差，则表述为式中为机器人连杆的加工误差，它由机械加工水平所决定。为环境温度所引起的连杆的长度变化，对于长度为的连杆，当环境温度变化量为时，其引起连杆长度变化量为式中40\n基于手脚融合的多足步行机器人的运动精度研究为连杆材料的热延伸系数。对于多足步行机器人，由于工作环境的不同，环境温度所引起的杆长变化量亦不同，在温差较大的环境中，该变化量可成为杆长误差的主要因素，在恒温状态下工作的机器人，该变化量则可以忽略。（2）立足点位置误差由于工作环境的非结构化，机器人立足点的位置会偏离预期的位置，立足点位置误差（）也会造成机器人机体位姿误差，还会对机器人的稳定性造成影响。立足点位置误差可表示为（3）机器人的关节运动变量误差机器人腿部转动关节的运动变量（）误差使机器人平台的位姿产生误差。导致关节的运动变量产生误差的主要静态因素有：1.电机控制系统的工作误差；2.传动机构的运动误差；3.传动机构的间隙。因此，由静态因素引起的机器人关节的运动变量误差可表示为式中，为电机控制系统的误差；为传动机构的误差；为传动机构的间隙。（4）机器人关节柔性误差机器人的关节一般存在一定的柔性，在机器人自身重力、外力、及惯性力作用下，使机器人平台产生位姿误差。机器人支撑腿各关节的柔性实际上将导致关节产生相应的运动变量误差（）表示，由关节的柔性所造成的运动变量误差的计算公式式中：为关节的刚度系数；为作用于机器人支撑腿上的外力矢量沿、、轴上的分量；为外力矢量的作用位置矢量。为作用于机器器人腿上的外力矢量数目。将支撑腿关节的柔性对机器人位姿误差的影响归结为机器人腿部关节运动变量误差可通过机器人静态位姿误差分析模型来计算由关节柔性引起的机器人平台的位置误差。（5）支撑腿连杆柔性误差焦国太[51]介绍了分析腿部连杆柔性所引起的机器人位姿误差的结构矩阵分析方法。应用该方法可求得机器人平台由腿部连杆所引起的位置和姿态误差，需要注意其姿态误差按照固定坐标系分别绕、、轴的微小转角给出，为统一使用欧拉角来表示机器人末端的姿态误差，必须对其进行变换。40\n基于手脚融合的多足步行机器人的运动精度研究假定在某时刻由机器人支撑腿连杆柔性所引起的机器人平台位姿误差为：，则由微分变化算子可得从而得到机器人机身的坐标系和固定坐标系之间的相对变换矩阵根据上式,求得考虑连杆柔性时机器人平台的实际位置和姿态，实际位姿和名义位姿的差即为经过变换后由腿部连杆柔性所引起的机器人平台位姿误差：将支撑腿连杆柔性所引起的机器人平台位姿误差进行转换后即可和由静态误差和关节柔性引起的机器人平台的误差进行迭加,得到机器人平台的综合位姿误差。由上述分析可知,多足步行机器人平台的位姿误差由很多因素引起,如果将这些因素作为孤立的因素,分别加以考虑,会使位姿误差的分析变得十分的复杂。可行的方法就是将所有因素引起的位姿误差都归结为支撑腿各连杆的运动变量误差引起的位姿误差、各组成连杆的结构参数误差引起的位姿误差以及立足点误差所引起的机器人位姿误差。从误差分析和传递的情况分析，机器人平台的位姿误差与支撑腿各组成连杆的运动变量、结构参量和立足点之间存在函数关系，就是将各因素引起的平台位姿误差归结为各组成连杆的D-H参数误差和立足点误差引起的，这样将简化多足步行机器人误差分析的模型，为误差分析带来方便。1.1机器人正运动学分析和误差分析2.4.1串联机械手的正运动学具有手脚融合功能的机械手由4个回转关节组成，包括：髋关节、大腿关节、小腿关节和腕关节。其关节结构简图如图2.3所示。机械手的正运动学就是已知机械手连杆参数和关节变量，求末端执行器相对于参考坐标系的位置和姿态。40\n基于手脚融合的多足步行机器人的运动精度研究图2.3机械手结构简图1324机体采用D-H法建立机械手坐标系，该方法通过在每个连杆上固定一个坐标系，用４阶的齐次变换矩阵描述两个杆件的空间关系（位置和姿态），从而推导出机械手末端坐标系相对于参考系的等价齐次变换矩阵．D-H坐标系规定：在机械手的各个主要构件上固定坐标系。坐标系的Z轴可与运动副的轴线重合，而X轴则沿着相邻两个Z轴的公垂线，至于Y轴可由右手坐标系法则来确定[58-59]。对回转连接的两杆件,在D-H方法中连杆构件坐标系的选择及参数的规定如下：由原点Oi和坐标轴Xi、Yi、Zi定义的坐标系Fi，Fi被固结在第i-1个连杆上，其中i=1,2,…,n+1。1)Zi坐标轴是沿着i+1关节方向的运动轴。2)Xi轴是沿着Zi-1和Zi的公垂线方向的坐标轴，方向是前者指向后者(见图2.4a)，如果Zi-1和Zi相交,Xi的正方向就不确定,可以任意指定。如果Zi-1和Zi平行(见图2.4c),Xi的位置就不确定。为保证定义唯一,规定Xi通过第i-1个坐标系的原点。3)公垂线ai是Zi-1和Zi两轴间的最小距离，一般ai称为连杆长度。图2.4D-H坐标系40\n基于手脚融合的多足步行机器人的运动精度研究4)两公垂线ai-1和ai之间的距离di，di称为连杆距离。5)Zi-1和Zi之间的夹角定义为,以绕Xi轴右旋为正,称为扭转角。6)Xi-1和Xi之间的夹角定义为θi,以绕Zi-1轴右旋为正,一般称θi为连杆的夹角。根据图2.3的关节布置及D-H法则，建立各连杆的坐标系，如图2.5所示。这里的参考坐标系固定在髋关节上，为固连于机械手手部的坐标系，根据以下原则确定的：图2.5机械手的参数坐标系取手部中心点为原点，关节轴方向的单位矢量为轴，手指连线方向的单位矢量为轴，轴则按右手法则来确定。根据所建立的连杆坐标系，可以确定连杆坐标参数，如表2-1所示。表2-1连杆参数杆件号关节转角（变量）扭角杆长距离102030040为求解运动学方程式，用齐次变换矩阵描述第i坐标系相对于第（i-1）坐标系的位置和方位，即连杆变换通式：（2-1）40\n基于手脚融合的多足步行机器人的运动精度研究由连杆变换通式及连杆参数可得到各连杆变换矩阵。为书写方便，令。各连杆变换矩阵为：根据图4-6，将各连杆变换矩阵相乘可得到末端执行器坐标系相对于的齐次变换阵。将各连杆变换矩阵代入，可求得（2-2）式中2.4.2机器人抓取时的正运动学分析A1A2A3B1B2B3B4图2.6机器人整体坐标系40\n基于手脚融合的多足步行机器人的运动精度研究建立机器人在抓取物体时的整体坐标系，如图2.6所示。其中为参考坐标系，为固定在机器人机体上、原点与机器人几何中心重合的坐标系，为机身与髋关节连接处的坐标系，为末端执行器坐标系。由所建立的坐标系可以得到各坐标系的相对齐次变换矩阵。由图2.6可以得到各坐标系的相对齐次变换矩阵。(2-3)(2-4)末端执行器相对于的齐次变换阵，即机器人在抓取物体时的运动学方程为：（2-5）根据所建立的坐标系可以求得末端执行器坐标系相对地面坐标系的齐次变换矩阵，该矩阵描述了末端执行器在地面坐标系的位置和姿态。即机器人在抓取物体时的运动学方程为：（2-6）将（2-2）、（2-3）、（2-4）代入（2-6）式，可以求得末端执行器在地面坐标系的位姿。(2-7)式中40\n基于手脚融合的多足步行机器人的运动精度研究通过对正运动学分析所得的方程微分求出误差方程为：40\n基于手脚融合的多足步行机器人的运动精度研究2.4.3算例在此我们在MATLAB中调用名为forward_hand的M文件，其中Theta，il，Rc，dRc，m，n，d4，d_Theta，P_0_c，d_P_0_c，dm,dn，idl为输入参数，其中Theta为连杆夹角il是一个的矩阵，表示多足步行机器人腿长的理论值；Rc是一个的矩阵，表示多足步行机器人机体动平台的状态，其意义式为m,n为机体尺寸40\n基于手脚融合的多足步行机器人的运动精度研究d4表示第四根杆的距离P_0_c是一个的矩阵，表示多足步行机器人质心点的理论位置，意义为；下面用一组数据为例，并以此做为误差分析的一种在理论上的证明：调用附录1中的M文件中的函数，其输出参数的结果会出现在命令窗体中，输入如下参数：Theta=[0,0,0;0,-30,90;0,0,0;0,0,0];il=[65,200,266];d4=180;Rc=[1,0,0;0,1,0;0,0,1];dRc=[0.30,0,0;0,0.50,0;0,0,0.20];d_Theta=[0,0,0.40,0];P_0_c=[-40,80,266];m=197;n=140;d_P_0_c=[0,0,0.20];idl=[0,0.75,0];dm=2;dn=1;forward_hand(Theta,il,Rc,dRc,m,n,d4,d_Theta,P_0_c,d_P_0_c,dm,dn,idl);得出的结果如下：解得的手的位置如下0.4268-0.8515-0.3048171.7707000220.00000.1366-0.27250.9524716.94820001.0000机械臂正运动学误差d_F=d_F=0.0734-0.4017-0.472454.0373000000.28610.068652.602840\n基于手脚融合的多足步行机器人的运动精度研究以上结果显示了正运动学方程在MATLAB的计算中的误差的正确性1.1本章小结本章首先对基于手脚融合的多足步行机器人做了简单的介绍，并分析了多足步行机器人的误差产生的原因主要包括机器人零部件的加工制造误差，机器人的安装，传动机构的误差，机器人连杆和关节的柔性及机器人工作环境等因素。详细介绍了几种对多足步行机器人运动精度影响最大的误差因素。最后我们介绍了机器人串联手的正运动学，对机器人抓取时的正运动学分析做了详细分析，导出机器人正运动学误差方程，并通过实例验证误差方程的正确性。40\n基于手脚融合的多足步行机器人的运动精度研究多足步行机器人的逆运动学分析和误差分析1.1机器人的逆运动学分析和误差分析在第2章中我们介绍了机器人正运动学的运动学分析和误差分析的推导方程，从工程应用角度来说，运动学逆解更为重要，它是机器人运动规划和轨迹控制的基础。正向运动学是唯一确定的，即各个关节变量给定之后，足端的位姿是唯一确定的；然而运动学逆解往往是很多种解，也可能不存在解。该机器人的逆运动学分析是根据机器手所抓取物体的位姿，求机器人的机体的位姿、各站立腿足端的位置，以及机器手与站立腿各关节转角。本章我们将介绍逆运动学的运动学分析和误差方程的推导方法。当给定机械手所抓取物体的位姿，求机器人机体的位姿、各站立腿足端的位置，以及机器手与站立腿各关节转角时，将有众多解。在实际设计中，可以根据规划的步态轨迹，事先确定该机器人机体的位姿和，以及机器人立足点的位置，求机械手各关节转角。运动学逆问题求解方法是：将运动学方程式的两端依次左乘各矩阵的逆矩阵，并使两端相等矩阵的对应元素相等，就可求得各关节变量。具体求解步骤如下：用逆变换左乘方程（2-6）两边得：（3-1）（3-2）（3-3）（3-4）40\n基于手脚融合的多足步行机器人的运动精度研究（3-5）把(3-2)(3-3)(3-4)(3-5)代入方程（3-1）左端可得：式中方程（3-1）右端为式中1)求解和根据方程（3-1）两边元素（3，4）对应相等，可得利用三角代换（3-6）式中：式中：40\n基于手脚融合的多足步行机器人的运动精度研究由以上可知：进行三角代换后可得所以：式中：对（3-6）两边求导可得到（3-7）式中：2）求解和根据方程（3-1）两边元素（2，3）对应相等，可求得：(3-8)(3-9)式中对（3-9）两边求导可得：(3-10)40\n基于手脚融合的多足步行机器人的运动精度研究3）求解和根据方程（3-1）两边元素（2，4对应相等，可得）：（3-11）联立（3-8）代入（3-11）可得（3-12）（3-13）（3-14）对（3-12）两边求导可得：(3-15)4）求解和把式（3-14）代入（3-9）可得：(3-16)5）求解和根据方程（3-1）两边元素（3，1）（3，2）对应相等，可得：（3-17）式中(3-18)式中40\n基于手脚融合的多足步行机器人的运动精度研究1.1算例图3-1手脚融合四足机器人结构简图图3-2机器人在中平面上的投影图如图3-1所示，一个四足机器人沿轴方向步行，其机体与地面保持平行。假定中的轴与中的轴平行。四条腿标记为,，及。其中腿设计为机械手，，，，，，。根据步态设计，机器人机体的位置。手部的位姿为：。id_P_0_A是一个的矩阵，表示多足步行机器人足端在利用MATLAB编辑程序计算机器人逆运动学的过程，我采用编辑M文件，建立主函数的方法。在此我们以liuboni为函数名的M文件，其中il，P_0_c，Rc，zq_T，d4，m，n，P_0_A，d_Rc，d_P_0_c，d_d4，id_l，d_ZqT，dm，dn为输入参数，其中il是一个的矩阵，表示多足步行机器人腿长的理论值；P_0_c是一个的矩阵，表示多足步行机器人质心点的理论位置，意义为；m,n为机体尺寸Rc是一个的矩阵，表示多足步行机器人机体动平台的状态，其意义式为40\n基于手脚融合的多足步行机器人的运动精度研究zq_T为的表达是为下面用一组数据为例，并以此做为误差分析的一种在理论上的证明：调用附录2中的M文件中的函数，其输出参数的结果会出现在命令窗体中，输入如下参数：il=[0.03,0.10,0.25];d4=0.10;Rc=[1,0,0;0,1,0;0,0,1];d_Rc=[0,0,0;0,0,0;0,0,0];zq_T=[0,1,0,0.3;1,0,0,0.45;0,0,-1,0.07;0,0,0,1];d_ZqT=[0.01,0,0,0;0,0,0,0;0,0,0,0;0,0,0,0];P_0_A=[0.350.200.07];P_0_c=[0,0.25,0.25];d_P_0_c=[0.050.050.05];d_d4=0.05;m=0.1;n=0.2;id_l=[0.05,0.05,0.05];dm=0;dn=0;liuboni(il,P_0_c,Rc,zq_T,d4,m,n,P_0_A,d_Rc,d_P_0_c,d_d4,id_l,d_ZqT,dm,dn);得出的结果如下：抓取髋关节角度:s1=-45.0000抓取臂大腿关节角度:s2=-53.1301抓取臂小腿关节角度:s3=53.1301抓取臂小腿旋转关节角度:s4=45d_Theta1=-0.2255d_Theta2=1.1809d_Theta3=-1.1809d_Theta4=-0.010040\n基于手脚融合的多足步行机器人的运动精度研究以上结果显示了逆运动学方程在MATLAB的计算中的误差的正确性1.1本章小结本章主要分析多足步行机器人的逆运动学方程最后导出逆运动学的误差方程。并通过实例验证所导出的逆运动学误差方程的正确性。40\n基于手脚融合的多足步行机器人的运动精度研究结论步行机器人需要根据机器人机体与参考基础之间复杂的非线性几何关系求得关节角。然后，通过关节电机伺服，驱动关节至相应的位置。然而，关节之间名义的几何参数(包括结构参数和运动输入变量)，由于制造装配误差、环境温度变化、电机控制系统的误差、传动机构的运动误差等原因，很可能是不精确的。这样，机器人控制器所使用的运动学模型与真实的运动学模型不一致，按理想的运动学模型驱动关节电机获得的实际机器人机体的位姿将有可能偏离要求的位姿。步行机器人机体位姿的运动误差也会受到非几何参数误差的影响。如关节和连杆的弹性变形、振动、回差等。在本文中，主要研究驱动关节变量的误差对机器人机体位姿误差的影响。实际上，驱动关节变量的误差可以含盖测量系统的误差、制造装配误差、环境温度变化、电机控制系统的误差、传动机构的运动误差、传动机构的间隙、关节和连杆的弹性变形、振动、回差等的影响。根据希望的步态轨迹确定机器人各驱动关节控制变量的过程是机器人运动学计算中的逆运动学问题。本文中，在论述多足步行机器人逆运动学计算过程的基础上，研究了多足步行机器人轨迹精度与驱动关节变量运动精度之间的关系。依据机器人的结构及运动约束关系，获得了机器人的误差表达式，包含机器人腿的站立点在机体坐标系及臂关节坐标系中的运动误差、方向矩阵的运动误差、站立腿驱动关节参数误差表达等。导出了表示驱动关节参数误差、结构参数、机器人轨迹误差相互关系的方程组。在本文的实验中，根据机器人的理论模型设计一条直线步态，通过对机器人在该步态下实际运动中机体轨迹误差数据的测量，通过机器人的误差模型，计算出站立腿驱动关节参数误差值。将该误差值的负值作为驱动关节参数的补偿量，按补偿后的关节参数重新驱动多足步行机器人运动该步态，实验结果表明：机器人机体轨迹的运动精度显著提高。40\n基于手脚融合的多足步行机器人的运动精度研究致谢本文的研究工作是在导师王良文教授的悉心指导和亲切关怀下完成的，导师严谨的学风，渊博的学识，和一丝不苟的工作态度使我在工作、学习、做人等方面深受启迪和教诲，这将使我终身受益。在此，谨向导师表示衷心的感谢和崇高的敬意。在整个毕业设计过程中，王老师给我提出了明确的思路，使我在整个设计过程中少走了许多弯路，为毕业设计的顺利完成奠定了坚实的基础。在整个毕业设计的过程中，我也从中学到了很多的东西，特别是对误差结果的分析，解决实验过程中出现的问题等使我受益良多。在本文的完成过程中我要特别感谢在我的实验过程中给予我帮助过的学长唐唯刚和学长张小辉，同学张宝轻，姚德波，何彩繁，王朋，陈瑞峰，公丕兵，胡亚辉。最后，要深深地感谢我深爱的父母以及家人，是他们在精神上、经济上给予的莫大鼓励与支持，使我顺利完成本科学习阶段的学习工作。感谢朋友、同学们在本科学习阶段给予我莫大的关心与支持。谨以此文献给所有关心、帮助和支持过我的人们！40\n基于手脚融合的多足步行机器人的运动精度研究参考文献[1]黄俊军，葛世荣，曹为.多足步行机器人研究状况及展望[J].机床与液压，2008，(5).[2]S.Hirose,Y.Umetani.SomeConsiderationonaFeasibleWalkingMechanismasaTerrainVehicle[C].Pcocs.of3rdRoManSySymp.Udine,Italy,1978.357-375[3]H.Kimura,I.ShimoyamaandH.Miura，＂CriteriaforDynamic　WalkoftheQuadruPed＂[C].19th　IntemationalSymPosiumoflndustrialRobot，Sydney，1988:595-600[4]S.Hirose,T.Masui,H.Kikuchi.TITAN-III:AQuadrupedWalkingVehicle–ItsStructureandBasicCharacteristic[C].RoboticResearch(2ndInt.Symp.),TheMITPress,1985.325-331[5]S.Hirose,K.Yoneda,H.Tsukagoshi.TITAN-VII:QuadrupedWalkingandManipulatingRobotonaSteepSlope[C].Proc.Int.Conf.onroboticsandautomation.Albuquerque,NewMexico,1997.494-500[6]K.Arikawa,S.Hirose.DevelopmentofQuadrupedWalkingRobotTITAN-VIII[C].Proc.ofthe1996IEEE/RSJInt.Conf.onIntelligentRobotsandSystems.Osaka,Japan,1996.208-214[7]M.Buehler,R.Bayyaglia,A.Cocoscoetal.SCOUT:ASimpleQuadrupedthatWalks,Climb,andRuns[C].IEEEInt.Conf.Roboticsandautomation.Leuven,Belgium.May1998.1701-1712[8]雷静桃,高峰,崔莹,.多足步行机器人的研究现状及展望[J].机械设计,2006,(9).[9]BernsK，IIgW，DeekM，etal.Themammalian-likequadrupedalwalkingmachineBISAM[C].19985thInternationalWorkshoPonAdvancedMotionControl，Gennany，1998:429-433.[10]阳如坤.全方位六足步行机器人分析腿机构、运动学、静力学分析、步态规划与步行软件[D].沈阳:中科院沈阳自动化研究所,1991.[11]马培荪,窦小红,刘臻.全方位四足步行机器人的运动学研究[J].上海交通大学学报,1994,28(2):36-39.[12]徐小云,颜国正,丁国清.微型六足仿生机器人及其三角步态的研究[J].光学精密工程,2002,10(4):392-396.[13]郭鸿勋.多足步行机器人机械系统模型的研究与设计[D].湖北武汉：华中科技大学，2004.4[14]苏军.多足步行机器人步态规划及控制的研究[D].湖北武汉：华中科技大学，2004.4[15]吕崇耀，熊有伦．Stewart并联机构位姿误差分析[J]．华中理工大学学报，2000，27(8)：4~6[16]邹豪，王启义．并联Stewart机构位姿误差分析[J]．东北大学学报，2000，21(3)：301~304[17]李嘉，王纪武，陈恳等．基于广义几何误差模型的微机器人精度分析[J]．机械工程学报，2000，36(8)：20~24[18]牛国栋．三平移并联机器人机构的精度研究：[硕士学位论文]．山东理工大学，200640\n基于手脚融合的多足步行机器人的运动精度研究[1]李凌云．Stewart机构的误差建模与参数标定方法研究：[硕士学位论文]．燕山大学，2006[2]崔道碧．关节间隙对机器人末端执行器位姿误差的影响[J]．湖南大学学报(自然科学版)，1999，26(2)：32~36[3]黄真，李艳文，高峰．空间运动构件姿态的欧拉角表示[J]．燕山大学学报，2002，26(3)：189~192[4]汪劲松，白杰文，高猛等．Stewart平台铰链间隙误差的精度分析[J]．清华大学学报(自然科学版)，2002，42(6)：758~761[5]徐卫良，张启先．机器人误差分析的蒙特卡洛方法[D]．机器人，1988，2(4)：1~6[6]洪林．并联机器人精度分析与综合研究：[博士学位论文]．天津大学，2004[7]焦国太，余跃庆，梁浩．机器人位姿误差的结构矩阵分析方法[J]．应用基础与工程科学学报，2001，9(2)：259~265[8]祃琳，黄田，王洋等．面向制造的并联机床精度设计[D]．中国机械工程，1999，10(10)：1114~111840\n基于手脚融合的多足步行机器人的运动精度研究附录附录1以下是正运动学运动方程和误差方程的MATLAB程序：function[]=forward_hand(Theta,il,Rc,dRc,m,n,d4,d_Theta,P_0_c,d_P_0_c,dm,dn,idl);index=2;d_d4=0;Theta(2,4)=-90;%I_jiaodu%I腿角度%il%各腿的长度%R_c机体位姿%c_x_bBI点的在C坐标中的位置%o_P_c质心在O坐标系中的坐标l1=il(1);l2=il(2);l3=il(3);d_l1=idl(1);d_l2=idl(2);d_l3=idl(3);P_c_B=[-m,n,0;...%Leg1m,n,0;...%Leg2-m,-n,0;...%Leg3m,-n,0]';%Leg4id_P_c_B=[-dm,dn0;...%Leg1dm,dn,0;...%Leg2-dm,-dn,0;...%Leg3dm,-dn,0]';%Leg4d_P_c_B=id_P_c_B;B_T_1=[cos(Theta(index,1)),0,sin(Theta(index,1)),l1*cos(Theta(index,1));sin(Theta(index,1)),0,-sin(Theta(index,1)),l1*sin(Theta(index,1));0,1,0,0;0,0,0,1];T_2=[cos(Theta(index,2)),-sin(Theta(index,2)),0,l2*cos(Theta(index,2));sin(Theta(index,2)),cos(Theta(index,2)),0,l2*sin(Theta(index,2));0,0,1,0;0,0,0,1];T_3=[cos(Theta(index,3)),0,sin(Theta(index,3)),0;sin(Theta(index,3)),0,-cos(Theta(index,3)),0;0,1,0,0;0,0,0,1];T_AI=[cos(Theta(index,4)),-sin(Theta(index,4)),0,0;sin(Theta(index,4)),cos(Theta(index,4)),0,0;0,0,1,l3;0,0,0,1];B_T_A1=B_T_1*T_2*T_3*T_AI;C_T_BI=[1,0,0,P_c_B(1,index);0,1,0,P_c_B(2,index);0,0,1,P_c_B(3,index);0,0,0,1];O_T_C=[Rc(1,1),Rc(1,2),Rc(1,3),P_0_c(1);Rc(2,1),Rc(2,2),Rc(2,3),P_0_c(2);Rc(3,1),Rc(3,2),Rc(3,3),P_0_c(3);0,0,0,1];O_T_AI=O_T_C*C_T_BI*B_T_A1;disp('解得的手的位置如下');disp(O_T_AI);40\n基于手脚融合的多足步行机器人的运动精度研究d_f11=dRc(1,1)*(cos(Theta(index,1))*cos(Theta(index,2)+Theta(index,3))*cos(Theta(index,4))+sin(Theta(index,1))*sin(Theta(index,4)))-Rc(1,1)*(sin(Theta(index,1)*d_Theta(1)*cos(Theta(index,2)+Theta(index,3))*cos(Theta(index,4))+cos(Theta(index,1))*sin(Theta(index,2)+Theta(index,3))*(d_Theta(2)+d_Theta(3))*cos(Theta(index,4))+cos(Theta(index,1))*cos(Theta(index,2)+Theta(index,3))*sin(Theta(index,4))*d_Theta(4)-cos(Theta(index,1))*d_Theta(1)*sin(Theta(index,4))-sin(Theta(index,1))*cos(Theta(index,4))*d_Theta(4))+dRc(1,2)*(sin(Theta(index,1))*cos(Theta(index,2)+Theta(index,3))*cos(Theta(index,4))-cos(Theta(index,1))*sin(Theta(index,4)))+Rc(1,2)*(cos(Theta(index,1))*d_Theta(1)*cos(Theta(index,2)+Theta(index,3))*cos(Theta(index,4))-sin(Theta(index,1))*sin(Theta(index,2)+Theta(index,3))*(d_Theta(2)+d_Theta(3))*cos(Theta(index,4))-sin(Theta(index,1))*cos(Theta(index,2)+Theta(index,3))*sin(Theta(index,4))*d_Theta(4)+sin(Theta(index,1))*d_Theta(1)*sin(Theta(index,4))-cos(Theta(index,1))*cos(Theta(index,4))*d_Theta(4))+dRc(1,3)*sin(Theta(index,2)+Theta(index,3))*cos(Theta(index,4))+Rc(1,3)*cos(Theta(index,2)+Theta(index,3))*(d_Theta(2)+d_Theta(3))*cos(Theta(index,4))+Rc(1,3)*sin(Theta(index,2)+Theta(index,3))*sin(Theta(index,4))*d_Theta(4));%注意要给定Theta(index,4)和d_Theta(4)的值d_f12=dRc(1,1)*(-cos(Theta(index,1)*cos(Theta(index,2)+Theta(index,3))*sin(Theta(index,4))+sin(Theta(index,1))*cos(Theta(index,4)))+Rc(1,1)*(sin(Theta(index,1))*d_Theta(1)*cos(Theta(index,2)+Theta(index,3))*sin(Theta(index,4))+cos(Theta(index,1))*sin(Theta(index,2)+Theta(index,3))*(d_Theta(2)+d_Theta(3))*sin(Theta(index,4))-cos(Theta(index,1))*cos(Theta(index,2)+Theta(index,3))*cos(Theta(index,4))*d_Theta(4)+cos(Theta(index,1))*d_Theta(1)*cos(Theta(index,4))-sin(Theta(index,1))*sin(Theta(index,4))*d_Theta(4))-dRc(1,2)*sin(Theta(index,1))*cos(Theta(index,2)+Theta(index,3))*sin(Theta(index,4))+cos(Theta(index,1))*cos(Theta(index,4)))-Rc(1,2)*(cos(Theta(index,1))*d_Theta(1)*cos(Theta(index,2)+Theta(index,3))*sin(Theta(index,4))-sin(Theta(index,1))*sin(Theta(index,2)+Theta(index,3))*(d_Theta(2)+d_Theta(3))*sin(Theta(index,4))+sin(Theta(index,1))*cos(Theta(index,2)+Theta(index,3))*cos(Theta(index,4))*d_Theta(4)-sin(Theta(index,1))*d_Theta(1)*cos(Theta(index,4))-cos(Theta(index,1))*sin(Theta(index,4))*d_Theta(4))-dRc(1,3)*sin(Theta(index,2)+Theta(index,3))*sin(Theta(index,4))+Rc(1,3)*cos(Theta(index,2)+Theta(index,3))*(d_Theta(2)+d_Theta(3))*sin(Theta(index,4))-Rc(1,3)*sin(Theta(index,2)+Theta(index,3))*cos(Theta(index,4))*d_Theta(4);d_f13=dRc(1,1)*cos(Theta(index,1))*sin(Theta(index,2)+Theta(index,3))-Rc(1,1)*sin(Theta(index,1))*d_Theta(1)*sin(Theta(index,2)+Theta(index,3))+Rc(1,1)*cos(Theta(index,1))*cos(Theta(index,2)+Theta(index,3))*(d_Theta(2)+d_Theta(3))+dRc(1,2)*sin(Theta(index,1))*sin(Theta(index,2)+Theta(index,3))+Rc(1,2)*cos(Theta(index,1))*d_Theta(1)*sin(Theta(index,2)+Theta(index,3))+Rc(1,2)*sin(Theta(index,1))*cos(Theta(index,2)+Theta(index,3))*(d_Theta(2)+d_Theta(3))-dRc(1,3)*cos(Theta(index,2)+Theta(index,3))+Rc(1,3)*sin(Theta(index,2)+Theta(index,3))*(d_Theta(2)+d_Theta(3));40\n基于手脚融合的多足步行机器人的运动精度研究d_f14=dRc(1,1)*(d4*cos(Theta(index,1))*sin(Theta(index,2)+Theta(index,3))+l2*cos(Theta(index,1))*cos(Theta(index,2))+l1*cos(Theta(index,1))+P_c_B(1,index)+P_0_c(1))+Rc(1,1)*(d_d4*cos(Theta(index,1))*sin(Theta(index,2)+Theta(index,3))-d4*sin(Theta(index,1))*d_Theta(1)*sin(Theta(index,2)+Theta(index,3))+d4*cos(Theta(index,1))*cos(Theta(index,2)+Theta(index,3))*(d_Theta(2)+d_Theta(3))+d_l2*cos(Theta(index,1))*cos(Theta(index,2))-l2*sin(Theta(index,1))*d_Theta(1)*cos(Theta(index,2))-l2*cos(Theta(index,1))*sin(Theta(index,2))*d_Theta(2)+d_l1*cos(Theta(index,1))-l1*sin(Theta(index,1))*d_Theta(1)+d_P_c_B(1,index)+d_P_0_c(1))+dRc(1,2)*(sin(Theta(index,1))*sin(Theta(index,2)+Theta(index,3))*d4+sin(Theta(index,1))*l2*cos(Theta(index,2))+l1*sin(Theta(index,1)))+Rc(1,2)*(cos(Theta(index,1))*d_Theta(1)*sin(Theta(index,2)+Theta(index,3))*d4+sin(Theta(index,1))*cos(Theta(index,2)+Theta(index,3))*(d_Theta(2)+d_Theta(3))*d4+sin(Theta(index,1))*sin(Theta(index,2)+Theta(index,3))*d_d4+cos(Theta(index,1))*d_Theta(1)*l2*cos(Theta(index,2))-sin(Theta(index,1))*l2*sin(Theta(index,2))*d_Theta(2)+d_l1*sin(Theta(index,1))+l1*cos(Theta(index,1))*d_Theta(1))+dRc(1,3)*(-cos(Theta(index,2)+Theta(index,3))*d4+l2*sin(Theta(index,2)))+Rc(1,3)*(sin(Theta(index,2)+Theta(index,3))*(d_Theta(2)+d_Theta(3))*d4-cos(Theta(index,2)+Theta(index,3))*d_d4+d_l2*sin(Theta(index,2))+l2*cos(Theta(index,2))*d_Theta(2))+dRc(1,1)*P_c_B(1,index)+Rc(1,1)*d_P_c_B(1,index)+dRc(1,2)*P_c_B(2,index)+Rc(1,2)*d_P_c_B(2,index)+dRc(1,3)*P_c_B(3,index)+Rc(1,3)*d_P_c_B(3,index)+d_P_0_c(1);d_f21=dRc(2,1)*(cos(Theta(index,1))*cos(Theta(index,2)+Theta(index,3))*cos(Theta(index,4))+sin(Theta(index,1))*sin(Theta(index,4)))-Rc(2,1)*(sin(Theta(index,1)*d_Theta(1)*cos(Theta(index,2)+Theta(index,3))*cos(Theta(index,4))+cos(Theta(index,1))*sin(Theta(index,2)+Theta(index,3))*(d_Theta(2)+d_Theta(3))*cos(Theta(index,4))+cos(Theta(index,1))*cos(Theta(index,2)+Theta(index,3))*sin(Theta(index,4))*d_Theta(4)-cos(Theta(index,1))*d_Theta(1)*sin(Theta(index,4))-sin(Theta(index,1))*cos(Theta(index,4))*d_Theta(4))+dRc(2,2)*(sin(Theta(index,1))*cos(Theta(index,2)+Theta(index,3))*cos(Theta(index,4))-cos(Theta(index,1))*sin(Theta(index,4)))+Rc(2,2)*(cos(Theta(index,1))*d_Theta(1)*cos(Theta(index,2)+Theta(index,3))*cos(Theta(index,4))-sin(Theta(index,1))*sin(Theta(index,2)+Theta(index,3))*(d_Theta(2)+d_Theta(3))*cos(Theta(index,4))-sin(Theta(index,1))*cos(Theta(index,2)+Theta(index,3))*sin(Theta(index,4))*d_Theta(4)+sin(Theta(index,1))*d_Theta(1)*sin(Theta(index,4))-cos(Theta(index,1))*cos(Theta(index,4))*d_Theta(4))+dRc(2,3)*sin(Theta(index,2)+Theta(index,3))*cos(Theta(index,4))+Rc(2,3)*cos(Theta(index,2)+Theta(index,3))*(d_Theta(2)+d_Theta(3))*cos(Theta(index,4))+Rc(2,3)*sin(Theta(index,2)+Theta(index,3))*sin(Theta(index,4))*d_Theta(4));d_f22=dRc(2,1)*(-cos(Theta(index,1)*cos(Theta(index,2)+Theta(index,3))*sin(Theta(index,4))+sin(Theta(index,1))*cos(Theta(index,4)))+Rc(2,1)*(sin(Theta(index,1))*d_Theta(1)*cos(Theta(index,2)+Theta(index,3))*sin(Theta(index,4))+cos(Theta(index,1))*sin(Theta(index,2)+Theta(index,3))*(d_Theta(2)+d_Theta(3))*sin(Theta(index,4))-cos(Theta(index,1))*cos(Theta(index,2)+Theta(index,3))*cos(Theta(index,4))*d_Theta(4)+cos(Theta(index,1))*d_Theta(1)*cos(Theta(index,4))-sin(Theta(index,1))*sin(Theta(index,4))*d_Theta(4))-dRc(2,2)*sin(Theta(index,1))*cos(Theta(index,2)+Theta(index,3))*sin(Theta(index,4))+cos(Theta(index,1))*cos(Theta(index,4)))-Rc(2,2)*(cos(Theta(index,1))*d_Theta(1)*cos(Theta(index,2)+Theta(index,3))*sin(Theta(index,4))-sin(Theta(index,1))*sin(Theta(index,2)+Theta(index,3))*(d_Theta(2)+d_Theta(3))*sin(Theta(index,4))+sin(Theta(index,1))*cos(Theta(index,2)+Theta(index,3))*cos(Theta(index,4))*d_Theta(4)-sin(Theta(index,1))*d_Theta(1)*cos(Theta(index,4))-cos(Theta(index,1))*sin(Theta(index,4))*d_Theta(4))-dRc(2,3)*sin(Theta(index,2)+Theta(index,3))*sin(Theta(index,4))+Rc(2,3)*cos(Theta(index,2)+Theta(index,3))*(d_Theta(2)+d_Theta(3))*sin(Theta(index,4))-Rc(2,3)*sin(Theta(index,2)+Theta(index,3))*cos(Theta(index,4))*d_Theta(4);d_f23=dRc(2,1)*cos(Theta(index,1))*sin(Theta(index,2)+Theta(index,3))-Rc(2,1)*sin(Theta(index,1))*d_Theta(1)*sin(Theta(index,2)+Theta(index,3))+Rc(2,1)*cos(Theta(index,1))*cos(Theta(index,2)+Theta(index,3))*(d_Theta(2)+d_Theta(3))+dRc(2,2)*sin(Theta(index,1))*sin(Theta(index,2)+Theta(index,3))+Rc(2,2)*cos(Theta(index,1))*d_Theta(1)*sin(Theta(index,2)+Theta(index,3))+Rc(2,2)*sin(Theta(index,1))*cos(Theta(index,2)+Theta(index,3))*(d_Theta(2)+d_Theta(3))-dRc(2,3)*cos(Theta(index,2)+Theta(index,3))+Rc(2,3)*sin(Theta(index,2)+Theta(index,3))*(d_Theta(2)+d_Theta(3));40\n基于手脚融合的多足步行机器人的运动精度研究d_f24=dRc(2,1)*(d4*cos(Theta(index,1))*sin(Theta(index,2)+Theta(index,3))+l2*cos(Theta(index,1))*cos(Theta(index,2))+l1*cos(Theta(index,1))+P_c_B(1,index)+P_0_c(1)+Rc(2,1)*(d_d4*cos(Theta(index,1))*sin(Theta(index,2)+Theta(index,3))-d4*sin(Theta(index,1))*d_Theta(1)*sin(Theta(index,2)+Theta(index,3))+d4*cos(Theta(index,1))*cos(Theta(index,2)+Theta(index,3))*(d_Theta(2)+d_Theta(3))+d_l2*cos(Theta(index,1))*cos(Theta(index,2))-l2*sin(Theta(index,1))*d_Theta(1)*cos(Theta(index,2))-l2*cos(Theta(index,1))*sin(Theta(index,2))*d_Theta(2)+d_l1*cos(Theta(index,1))-l1*sin(Theta(index,1))*d_Theta(1)+d_P_c_B(1,index)+d_P_0_c(1))+dRc(2,2)*(sin(Theta(index,1))*sin(Theta(index,2)+Theta(index,3))*d4+sin(Theta(index,1))*l2*cos(Theta(index,2))+l1*sin(Theta(index,1)))+Rc(2,2)*(cos(Theta(index,1))*d_Theta(1)*sin(Theta(index,2)+Theta(index,3))*d4+sin(Theta(index,1))*cos(Theta(index,2)+Theta(index,3))*(d_Theta(2)+d_Theta(3))*d4+sin(Theta(index,1))*sin(Theta(index,2)+Theta(index,3))*d_d4+cos(Theta(index,1))*d_Theta(1)*l2*cos(Theta(index,2))-sin(Theta(index,1))*l2*sin(Theta(index,2))*d_Theta(2)+d_l1*sin(Theta(index,1))+l1*cos(Theta(index,1))*d_Theta(1))+dRc(2,3)*(-cos(Theta(index,2)+Theta(index,3))*d4+l2*sin(Theta(index,2)))+Rc(2,3)*(sin(Theta(index,2)+Theta(index,3))*(d_Theta(2)+d_Theta(3))*d4-cos(Theta(index,2)+Theta(index,3))*d_d4+d_l2*sin(Theta(index,2))+l2*cos(Theta(index,2))*d_Theta(2))+dRc(2,1)*P_c_B(1,index)+Rc(2,1)*d_P_c_B(1,index)+dRc(2,2)*P_c_B(2,index)+Rc(2,2)*d_P_c_B(2,index)+dRc(2,3)*P_c_B(3,index)+Rc(2,3)*d_P_c_B(3,index)+d_P_0_c(1));d_f31=dRc(3,1)*(cos(Theta(index,1))*cos(Theta(index,2)+Theta(index,3))*cos(Theta(index,4))+sin(Theta(index,1))*sin(Theta(index,4)))-Rc(3,1)*(sin(Theta(index,1)*d_Theta(1)*cos(Theta(index,2)+Theta(index,3))*cos(Theta(index,4))+cos(Theta(index,1))*sin(Theta(index,2)+Theta(index,3))*(d_Theta(2)+d_Theta(3))*cos(Theta(index,4))+cos(Theta(index,1))*cos(Theta(index,2)+Theta(index,3))*sin(Theta(index,4))*d_Theta(4)-cos(Theta(index,1))*d_Theta(1)*sin(Theta(index,4))-sin(Theta(index,1))*cos(Theta(index,4))*d_Theta(4))+dRc(3,2)*(sin(Theta(index,1))*cos(Theta(index,2)+Theta(index,3))*cos(Theta(index,4))-cos(Theta(index,1))*sin(Theta(index,4)))+Rc(3,2)*(cos(Theta(index,1))*d_Theta(1)*cos(Theta(index,2)+Theta(index,3))*cos(Theta(index,4))-sin(Theta(index,1))*sin(Theta(index,2)+Theta(index,3))*(d_Theta(2)+d_Theta(3))*cos(Theta(index,4))-sin(Theta(index,1))*cos(Theta(index,2)+Theta(index,3))*sin(Theta(index,4))*d_Theta(4)+sin(Theta(index,1))*d_Theta(1)*sin(Theta(index,4))-cos(Theta(index,1))*cos(Theta(index,4))*d_Theta(4))+dRc(3,3)*sin(Theta(index,2)+Theta(index,3))*cos(Theta(index,4))+Rc(3,3)*cos(Theta(index,2)+Theta(index,3))*(d_Theta(2)+d_Theta(3))*cos(Theta(index,4))+Rc(3,3)*sin(Theta(index,2)+Theta(index,3))*sin(Theta(index,4))*d_Theta(4));d_f32=dRc(3,1)*(-cos(Theta(index,1)*cos(Theta(index,2)+Theta(index,3))*sin(Theta(index,4))+sin(Theta(index,1))*cos(Theta(index,4)))+Rc(3,1)*(sin(Theta(index,1))*d_Theta(1)*cos(Theta(index,2)+Theta(index,3))*sin(Theta(index,4))+cos(Theta(index,1))*sin(Theta(index,2)+Theta(index,3))*(d_Theta(2)+d_Theta(3))*sin(Theta(index,4))-cos(Theta(index,1))*cos(Theta(index,2)+Theta(index,3))*cos(Theta(index,4))*d_Theta(4)+cos(Theta(index,1))*d_Theta(1)*cos(Theta(index,4))-sin(Theta(index,1))*sin(Theta(index,4))*d_Theta(4))-dRc(3,2)*sin(Theta(index,1))*cos(Theta(index,2)+Theta(index,3))*sin(Theta(index,4))+cos(Theta(index,1))*cos(Theta(index,4)))-Rc(3,2)*(cos(Theta(index,1))*d_Theta(1)*cos(Theta(index,2)+Theta(index,3))*sin(Theta(index,4))-sin(Theta(index,1))*sin(Theta(index,2)+Theta(index,3))*(d_Theta(2)+d_Theta(3))*sin(Theta(index,4))+sin(Theta(index,1))*cos(Theta(index,2)+Theta(index,3))*cos(Theta(index,4))*d_Theta(4)-sin(Theta(index,1))*d_Theta(1)*cos(Theta(index,4))-cos(Theta(index,1))*sin(Theta(index,4))*d_Theta(4))-dRc(3,3)*sin(Theta(index,2)+Theta(index,3))*sin(Theta(index,4))+Rc(3,3)*cos(Theta(index,2)+Theta(index,3))*(d_Theta(2)+d_Theta(3))*sin(Theta(index,4))-Rc(3,3)*sin(Theta(index,2)+Theta(index,3))*cos(Theta(index,4))*d_Theta(4);d_f33=dRc(3,1)*cos(Theta(index,1))*sin(Theta(index,2)+Theta(index,3))-Rc(3,1)*sin(Theta(index,1))*d_Theta(1)*sin(Theta(index,2)+Theta(index,3))+Rc(3,1)*cos(Theta(index,1))*cos(Theta(index,2)+Theta(index,3))*(d_Theta(2)+d_Theta(3))+dRc(3,2)*sin(Theta(index,1))*sin(Theta(index,2)+Theta(index,3))+Rc(3,2)*cos(Theta(index,1))*d_Theta(1)*sin(Theta(index,2)+Theta(index,3))+Rc(3,2)*sin(Theta(index,1))*cos(Theta(index,2)+Theta(index,3))*(d_Theta(2)+d_Theta(3))-dRc(3,3)*cos(Theta(index,2)+Theta(index,3))+Rc(3,3)*sin(Theta(index,2)+Theta(index,3))*(d_Theta(2)+d_Theta(3));40\n基于手脚融合的多足步行机器人的运动精度研究d_f34=dRc(3,1)*(d4*cos(Theta(index,1))*sin(Theta(index,2)+Theta(index,3))+l2*cos(Theta(index,1))*cos(Theta(index,2))+l1*cos(Theta(index,1))+P_c_B(1,index)+P_0_c(1))+Rc(3,1)*(d_d4*cos(Theta(index,1))*sin(Theta(index,2)+Theta(index,3))-d4*sin(Theta(index,1))*d_Theta(1)*sin(Theta(index,2)+Theta(index,3))+d4*cos(Theta(index,1))*cos(Theta(index,2)+Theta(index,3))*(d_Theta(2)+d_Theta(3))+d_l2*cos(Theta(index,1))*cos(Theta(index,2))-l2*sin(Theta(index,1))*d_Theta(1)*cos(Theta(index,2))-l2*cos(Theta(index,1))*sin(Theta(index,2))*d_Theta(2)+d_l1*cos(Theta(index,1))-l1*sin(Theta(index,1))*d_Theta(1)+d_P_c_B(1,index)+d_P_0_c(1))+dRc(3,2)*(sin(Theta(index,1))*sin(Theta(index,2)+Theta(index,3))*d4+sin(Theta(index,1))*l2*cos(Theta(index,2))+l1*sin(Theta(index,1)))+Rc(3,2)*(cos(Theta(index,1))*d_Theta(1)*sin(Theta(index,2)+Theta(index,3))*d4+sin(Theta(index,1))*cos(Theta(index,2)+Theta(index,3))*(d_Theta(2)+d_Theta(3))*d4+sin(Theta(index,1))*sin(Theta(index,2)+Theta(index,3))*d_d4+cos(Theta(index,1))*d_Theta(1)*l2*cos(Theta(index,2))-sin(Theta(index,1))*l2*sin(Theta(index,2))*d_Theta(2)+d_l1*sin(Theta(index,1))+l1*cos(Theta(index,1))*d_Theta(1))+dRc(3,3)*(-cos(Theta(index,2)+Theta(index,3))*d4+l2*sin(Theta(index,2)))+Rc(3,3)*(sin(Theta(index,2)+Theta(index,3))*(d_Theta(2)+d_Theta(3))*d4-cos(Theta(index,2)+Theta(index,3))*d_d4+d_l2*sin(Theta(index,2))+l2*cos(Theta(index,2))*d_Theta(2))+dRc(3,1)*P_c_B(1,index)+Rc(3,1)*d_P_c_B(1,index)+dRc(3,2)*P_c_B(2,index)+Rc(3,2)*d_P_c_B(2,index)+dRc(3,3)*P_c_B(3,index)+Rc(3,3)*d_P_c_B(3,index)+d_P_0_c(1);disp('机械臂正运动学误差d_F=');d_F=[d_f11,d_f12,d_f13,d_f14;d_f21,d_f22,d_f23,d_f23;d_f31,d_f32,d_f33,d_f34]附录2以下是逆运动学方程和误差方程的MTLAB程序function[]=liuboni(il,P_0_c,Rc,zq_T,d4,m,n,P_0_A,d_Rc,d_P_0_c,d_d4,id_l,d_ZqT,dm,dn);%手脚融合腿的逆运动学误差index=2;P_c_B=[-m,n,0;...%Leg1m,n,0;...%Leg2-m,-n,0;...%Leg3m,-n,0]';%Leg4id_P_c_B=[-dm,dn0;...%Leg1dm,dn,0;...%Leg2-dm,-dn,0;...%Leg3dm,-dn,0]';%Leg4d_P_c_B=id_P_c_B;l1=il(1);l2=il(2);l3=il(3);d_l1=id_l(1);d_l2=id_l(2);d_l3=id_l(3);s1=atan((Rc(1,2)*P_0_A(1)+Rc(2,2)*P_0_A(2)+Rc(3,2)*P_0_A(3)-Rc(1,2)*P_0_c(1)-Rc(2,2)*P_0_c(2)-Rc(3,2)*P_0_c(3)-P_c_B(2,index))/((Rc(1,1)*P_0_A(1)+Rc(2,1)*P_0_A(2)+Rc(3,1)*P_0_A(3)-Rc(1,1)*P_0_c(1)-Rc(2,1)*P_0_c(2)-Rc(3,1)*P_0_c(3)-P_c_B(1,index))));disp('抓取髋关节角度:s1=');disp(s1*180/pi);f12a=Rc(1,3)*zq_T(1,3)+Rc(2,3)*zq_T(2,3)+Rc(3,3)*zq_T(3,3);f12p=Rc(1,3)*P_0_A(1)+Rc(2,3)*P_0_A(2)+40\n基于手脚融合的多足步行机器人的运动精度研究Rc(3,3)*P_0_A(3)-Rc(1,3)*P_0_c(1)-Rc(2,3)*P_0_c(2)-Rc(3,3)*P_0_c(3)-P_c_B(3,index);s2=asin((f12p-d4*f12a)/l2);s23=acos(-f12a);s3=s23-s2;f13n=sin(s1)*(Rc(1,1)*zq_T(1,1)+Rc(2,1)*zq_T(2,1)+Rc(3,1)*zq_T(3,1))-cos(s1)*(Rc(1,2)*zq_T(1,1)+Rc(2,2)*zq_T(2,1)+Rc(3,2)*zq_T(3,1));f13o=sin(s1)*(Rc(1,1)*zq_T(1,2)+Rc(2,1)*zq_T(2,2)+Rc(3,1)*zq_T(3,2))-cos(s1)*(Rc(1,2)*zq_T(1,2)+Rc(2,2)*zq_T(2,2)+Rc(3,2)*zq_T(3,2));s4=atan(f13n/f13o);disp('抓取臂大腿关节角度:s2=');disp(s2*180/pi);disp('抓取臂小腿关节角度:s3=');disp(s3*180/pi);disp('抓取臂小腿旋转关节角度:s4=');disp(s4*180/pi);Theta(index,4)=s4;Theta(index,1)=s1;Theta(index,2)=s2;Theta(index,3)=s3;f11=Rc(1,1)*(cos(Theta(index,1))*cos(Theta(index,2)+Theta(index,3))*cos(Theta(index,4))+sin(Theta(index,1))*sin(Theta(index,4)))+Rc(1,2)*(sin(Theta(index,1))*cos(Theta(index,2)+Theta(index,3))*cos(Theta(index,4))+cos(Theta(index,1))*sin(Theta(index,4)))+Rc(1,3)*sin(Theta(index,2)+Theta(index,3))*cos(Theta(index,4));f12=Rc(1,1)*(-cos(Theta(index,1))*cos(Theta(index,2)+Theta(index,3))*sin(Theta(index,4))+sin(Theta(index,1))*cos(Theta(index,4)))-Rc(1,2)*(sin(Theta(index,1))*cos(Theta(index,2)+Theta(index,3))*sin(Theta(index,4))+cos(Theta(index,1))*cos(Theta(index,4)))-Rc(1,3)*sin(Theta(index,2)+Theta(index,3))*sin(Theta(index,4));f13=Rc(1,1)*cos(Theta(index,1))*sin(Theta(index,2)+Theta(index,3))+Rc(1,2)*sin(Theta(index,1))*sin(Theta(index,2)+Theta(index,3))-Rc(1,3)*cos(Theta(index,2)+Theta(index,3));f14=Rc(1,1)*(d4*cos(Theta(index,1))*sin(Theta(index,2)+Theta(index,3))+l2*cos(Theta(index,1))*cos(Theta(index,2))+l1*cos(Theta(index,1))+P_c_B(1,index)+P_0_c(1))+Rc(1,2)*(sin(Theta(index,1))*sin(Theta(index,2)+Theta(index,3))*d4)+sin(Theta(index,1))*l2*cos(Theta(index,2))+l1*sin(Theta(index,1))+Rc(1,3)*(-cos(Theta(index,2)+Theta(index,3))*d4+l2*sin(Theta(index,2)))+Rc(1,1)*P_c_B(1,index)+Rc(1,2)*P_c_B(2,index)+Rc(1,3)*P_c_B(3,index)+P_0_c(1);f21=Rc(2,1)*(cos(Theta(index,1))*cos(Theta(index,2)+Theta(index,3))*cos(Theta(index,4))+sin(Theta(index,1))*sin(Theta(index,4)))+Rc(2,2)*(sin(Theta(index,1))*cos(Theta(index,2)+Theta(index,3))*cos(Theta(index,4))+cos(Theta(index,1))*sin(Theta(index,4)))+Rc(2,3)*sin(Theta(index,2)+Theta(index,3))*cos(Theta(index,4));f22=Rc(2,1)*(-cos(Theta(index,1))*cos(Theta(index,2)+Theta(index,3))*sin(Theta(index,4))+sin(Theta(index,1))*cos(Theta(index,4)))-Rc(2,2)*(sin(Theta(index,1))*cos(Theta(index,2)+Theta(index,3))*sin(Theta(index,4))+cos(Theta(index,1))*cos(Theta(index,4)))-Rc(2,3)*sin(Theta(index,2)+Theta(index,3))*sin(Theta(index,4));40\n基于手脚融合的多足步行机器人的运动精度研究f23=Rc(2,1)*cos(Theta(index,1))*sin(Theta(index,2)+Theta(index,3))+Rc(2,2)*sin(Theta(index,1))*sin(Theta(index,2)+Theta(index,3))-Rc(2,3)*cos(Theta(index,2)+Theta(index,3));f24=Rc(2,1)*(d4*cos(Theta(index,1))*sin(Theta(index,2)+Theta(index,3))+l2*cos(Theta(index,1))*cos(Theta(index,2))+l1*cos(Theta(index,1))+P_c_B(1,index)+P_0_c(1))+Rc(2,2)*(sin(Theta(index,1))*sin(Theta(index,2)+Theta(index,3))*d4)+sin(Theta(index,1))*l2*cos(Theta(index,2))+l1*sin(Theta(index,1))+Rc(2,3)*(-cos(Theta(index,2)+Theta(index,3))*d4+l2*sin(Theta(index,2)))+Rc(2,1)*P_c_B(1,index)+Rc(2,2)*P_c_B(2,index)+Rc(2,3)*P_c_B(3,index)+P_0_c(1);f31=Rc(3,1)*(cos(Theta(index,1))*cos(Theta(index,2)+Theta(index,3))*cos(Theta(index,4))+sin(Theta(index,1))*sin(Theta(index,4)))+Rc(3,2)*(sin(Theta(index,1))*cos(Theta(index,2)+Theta(index,3))*cos(Theta(index,4))+cos(Theta(index,1))*sin(Theta(index,4)))+Rc(3,3)*sin(Theta(index,2)+Theta(index,3))*cos(Theta(index,4));f32=Rc(3,1)*(-cos(Theta(index,1))*cos(Theta(index,2)+Theta(index,3))*sin(Theta(index,4))+sin(Theta(index,1))*cos(Theta(index,4)))-Rc(3,2)*(sin(Theta(index,1))*cos(Theta(index,2)+Theta(index,3))*sin(Theta(index,4))+cos(Theta(index,1))*cos(Theta(index,4)))-Rc(3,3)*sin(Theta(index,2)+Theta(index,3))*sin(Theta(index,4));f33=Rc(3,1)*cos(Theta(index,1))*sin(Theta(index,2)+Theta(index,3))+Rc(3,2)*sin(Theta(index,1))*sin(Theta(index,2)+Theta(index,3))-Rc(3,3)*cos(Theta(index,2)+Theta(index,3));f34=Rc(3,1)*(d4*cos(Theta(index,1))*sin(Theta(index,2)+Theta(index,3))+l2*cos(Theta(index,1))*cos(Theta(index,2))+l1*cos(Theta(index,1))+P_c_B(1,index)+P_0_c(1))+Rc(3,2)*(sin(Theta(index,1))*sin(Theta(index,2)+Theta(index,3))*d4)+sin(Theta(index,1))*l2*cos(Theta(index,2))+l1*sin(Theta(index,1))+Rc(3,3)*(-cos(Theta(index,2)+Theta(index,3))*d4+l2*sin(Theta(index,2)))+Rc(3,1)*P_c_B(1,index)+Rc(3,2)*P_c_B(2,index)+Rc(3,3)*P_c_B(3,index)+P_0_c(1);d_Theta1=((2*(Rc(1,1)*f14+Rc(2,1)*f24+Rc(3,1)*f34-Rc(1,1)*P_0_c(1)-Rc(2,1)*P_0_c(2)-Rc(3,1)*P_0_c(3)-P_c_B(1,index))*(d_Rc(1,1)*f14+Rc(1,1)*d_ZqT(1,4)+d_Rc(2,1)*f24+Rc(2,1)*d_ZqT(2,4)+d_Rc(3,1)*f34+Rc(3,1)*d_ZqT(3,4)-d_Rc(1,1)*P_0_c(1)-Rc(1,1)*d_P_0_c(1)-d_Rc(2,1)*P_0_c(2)-Rc(2,1)*d_P_0_c(2)-d_Rc(3,1)*P_0_c(3)-Rc(3,1)*d_P_0_c(3)-d_P_c_B(1,index))+2*(Rc(1,2)*f14+Rc(2,2)*f24+Rc(3,2)*f34-Rc(1,2)*P_0_c(1)-Rc(2,2)*P_0_c(2)-Rc(3,2)*P_0_c(3)-P_c_B(2,index))*(d_Rc(1,2)*f14+Rc(1,2)*d_ZqT(1,4)+d_Rc(2,2)*f24+Rc(2,2)*d_ZqT(2,4)+d_Rc(3,2)*f34+Rc(3,2)*d_ZqT(3,4)-d_Rc(1,2)*P_0_c(1)-Rc(1,2)*d_P_0_c(1)-d_Rc(2,2)*P_0_c(2)-Rc(2,2)*d_P_0_c(2)-d_Rc(3,2)*P_0_c(3)-Rc(3,2)*d_P_0_c(3)-d_P_c_B(3,index)))/2*sqrt((Rc(1,1)*f14+Rc(2,1)*f24+Rc(3,1)*f34-Rc(1,1)*P_0_c(1)-Rc(2,1)*P_0_c(2)-Rc(3,1)*P_0_c(3)-P_c_B(1,index))^2+(Rc(1,2)*f14+Rc(2,2)*f24+Rc(3,2)*f34-Rc(1,2)*P_0_c(1)-Rc(2,2)*P_0_c(2)-Rc(3,2)*P_0_c(3)-P_c_B(2,index))^2)*cos(Theta(index,1))-d_Rc(1,1)*f14-Rc(1,1)*d_ZqT(1,4)-d_Rc(2,1)*f24-Rc(2,1)*d_ZqT(2,4)-d_Rc(3,1)*f34-Rc(3,1)*d_ZqT(3,4)+d_Rc(1,1)*P_0_c(1)+Rc(1,1)*d_P_0_c(1)+d_Rc(2,1)*P_0_c(2)+Rc(2,1)*d_P_0_c(2)+d_Rc(3,1)*P_0_c(3)+Rc(3,1)*d_P_0_c(3)+d_P_c_B(1,index))/(sqrt((Rc(1,1)*f14+Rc(2,1)*f24+Rc(3,1)*f34-Rc(1,1)*P_0_c(1)-Rc(2,1)*P_0_c(2)-Rc(3,1)*P_0_c(3)-P_c_B(1,index))^2+(Rc(1,2)*f14+Rc(2,2)*f24+Rc(3,2)*f34-Rc(1,2)*P_0_c(1)-Rc(2,2)*P_0_c(2)-Rc(3,2)*P_0_c(3)-P_c_B(2,index))^2)*sin(Theta(index,1)))d_Theta23=(d_Rc(1,3)*f13+Rc(1,3)*d_ZqT(1,3)+d_Rc(2,3)*f23+Rc(2,3)*d_ZqT(2,3)+d_Rc(3,3)*f33+Rc(3,3)*d_ZqT(3,3))*sqrt(1-(Rc(1,3)*f13+Rc(2,3)*f23+(Rc(3,3)*f33)^2)+(Rc(1,3)*f13+Rc(2,3)*f23+Rc(3,3)*f33)/2)*(d_Rc(1,3)*f13+Rc(1,3)*d_ZqT(1,3)+d_Rc(2,3)*f23+Rc(2,3)*d_ZqT(2,3)+d_Rc(3,3)*f33+Rc(3,3)*d_ZqT(3,3))*(1-(Rc(1,3)*f13+Rc(2,3)*f23+(Rc(3,3)*f33)^2)^(1/2));40\n基于手脚融合的多足步行机器人的运动精度研究d_Theta2=((d_Rc(1,3)*f14+Rc(1,3)*d_ZqT(1,4)+d_Rc(2,3)*f24+Rc(2,3)*d_ZqT(2,4)+d_Rc(3,3)*f34+Rc(3,3)*d_ZqT(3,4)-d_Rc(1,3)*P_0_c(1)-Rc(1,3)*d_P_0_c(1)-d_Rc(2,3)*P_0_c(2)-Rc(2,3)*d_P_0_c(2)-d_Rc(3,3)*P_0_c(3)-Rc(3,3)*d_P_0_c(3)-d_P_c_B(3,index))*l2-d_d4*(Rc(1,3)*f13+Rc(2,3)*f23+Rc(3,3)*f33)*l2-d4*(d_Rc(1,3)*f13+Rc(1,3)*d_ZqT(1,3)+d_Rc(2,3)*f23+Rc(2,3)*d_ZqT(2,3)+d_Rc(3,3)*f33+Rc(3,3)*d_ZqT(3,3))*l2-(Rc(1,3)*f14+Rc(2,3)*f24+Rc(3,3)*f34-Rc(1,3)*P_0_c(1)-Rc(2,3)*P_0_c(2)-Rc(3,3)*P_0_c(3)-P_c_B(3,index))*d_l2+d4*(Rc(1,3)*f13+Rc(2,3)*f23+Rc(3,3)*f33)*d_l2)/(l2^2)*cos(Theta(index,2))d_Theta3=d_Theta23-d_Theta2d_Theta4=(cos(Theta(index,1))*d_Theta1*(Rc(1,1)*f11+Rc(2,1)*f21+Rc(3,1)*f31)+sin(Theta(index,1))*(d_Rc(1,1)*f11+Rc(1,1)*d_ZqT(1,1)+d_Rc(2,1)*f21+Rc(2,1)*d_ZqT(2,1)+d_Rc(3,1)*f31+Rc(3,1)*d_ZqT(3,1))+sin(Theta(index,1))*d_Theta1*(Rc(1,2)*f11+Rc(2,2)*f21+Rc(3,2)*f31)-cos(Theta(index,1))*(d_Rc(1,2)*f11+Rc(1,2)*d_ZqT(1,1)+d_Rc(2,2)*f21+Rc(2,2)*d_ZqT(2,1)+d_Rc(3,2)*f31+Rc(3,2)*d_ZqT(3,1)))/cos(Theta(index,4))40