基于Matlab的双足步行机器人的腿部模型的建立及运动仿真

基于Matlab的双足步行机器人腿部运动模型的建立与运动仿真摘要：最近几年，双足仿人步行机器人发展很快，有很高的科学研究价值。步行机器人的运动是模仿人的步行运动的形式，相比其它机器人有更好的灵活性，所以可以完成各种生活中的难度更大的任务，实用价值远高于其它机器人，当然研究难度和控制也相当复杂。本设计主要研究了一些双足步行机器人的一些简单的问题，如自由度的配置，根据控制的难易选择驱动装置等。通过学习机器人学的基本理论，建立数学模型，运用MATLAB进行运动学的分析设计，做简单的运动模型的研究；然后用Pro/E建立一个简单的双足步行机器人的腿部的三维模型；最后根据之前的的分析和得出的运动数据，完成运动仿真。关键词：双足步行机器人；运动学；建模仿真IBasedonMatlabofbipedwalkingrobotlegmovementmodelandmovementsimulationAbstract：Thebipedwalkinghumanoidrobotisdevelopingquicklyinrecentyears,andhasaveryhighscientificvalue.ThemotionofWalkingrobotimitatesthewayofhumanswalking,hasbetterflexibilitythanothertypesofrobots,sothemoredifficultandpracticaltaskscanbecompleted,andthepracticalvalueismuchhigherthanthatofotherrobots.However,theresearchdifficultyandcontrolofthebipedwalkinghumanoidrobotisverycomplex.Thisdesignstudiessomefundamentalquestions,suchasDOFdisposition,thechoiceofdrivedeviceaccordingtothedifficultyofcontrolandsoon.ThroughthelearningofbasictheoryofRobotics，themathematicalmodelisestablished,theanalysisanddesignofsimplemovementpatternsUsingMATLABandtheresearchofsimplemovementpatternsiscompleted；Thenthethree-dimensionalmodelofthelegsofasimplebipedrobotwithPro/Esoftwareisestablished；Inthelast，accordingtothepreviousanalysisandmotiondata,themotionsimulationiscompleted.Keywords:Bipedwalkingrobot；Kinematics；3DSimulationII目录第一章前言.11.1课题背景及研究意义.11.2国内外研究现状.11.2.1国外研究状况.11.2.2国内研究状况.21.3双足步行机器人的腿部结构.21.4本论文研究的主要内容.31.5小结.3第二章双足机器人的模型建立.42.1双足机器人的腿部自由度.42.2机器人腿部的驱动方式.42.3腿部各个部位的尺寸与运动范围.52.3.1人体测量.52.3.2静态人体测量及肢体尺寸确定.62.3.3动态人体测量及运动范围确定.72.4小结.7第三章机器人学的基本相关理论.83.1坐标变换.83.1.1局部坐标系和齐次变换.83.1.2齐次变换的链乘法则.103.2转动特性.113.2.1侧摆、俯仰和转动.123.2.2旋转矩阵的说明.133.2.3旋转矩阵的性质.143.2.4角速度矢量.143.2.5旋转矩阵的微分与角速度矢量.163.2.6矩阵指数.173.3雅克比矩阵.18III3.4本章小结.19第四章双足机器人建模.204.1双足机器人数据结构的建立.204.1.1数据选择.204.1.2数据的遍历.214.2数学模型的构造.224.3正运动学.244.3.1三维矢量与反对称矩阵.244.3.2矩阵指数计算旋转矩阵.254.3.3正运动学计算.254.4逆运动学.264.4.1解析法求解逆运动学.264.4.2雅克比矩阵.284.5.1模拟步行数据计算与参数分析.294.5.2上体旋转一定角度行走.324.6小结.36第五章机器人模型仿真.385.1机器人腿部结构三维建模.385.1.1机器人腿部结构零件尺寸设计.385.1.2机器人腿部结构的组装与约束的确定.395.1.3驱动的添加.405.2极限位置仿真与模型修改.415.2.1上体竖直行走.415.2.2上体保持转动角度行走.425.3小结.44总结.44参考文献.46致谢.470第一章前言1.1课题背景及研究意义移动型的机器人的应用非常广泛，移动的方式也各不相同，主要的有履带式，轮式和步行式，其中，步行式有很大的优越性，适应环境的能力强，不管是相对狭隘的地方还是障碍性比较大的地方，它都可以越过，因此，步行式机器人的实际应用更为广泛。双足步行机器人的灵活性更好，其主要特点在于：首先，它对行走环境的要求不高，能适应各种地面。其次双足机器人具有广阔的工作空间，由于行走系统占地面积小，活动范围很大，机械手的配置使它具有更广阔的活动空间，难度最高的步行动作是双足步行，但其步行性的优越性与其它步行结构不可同日而语。因此，双足机器人的研究工作可以有力的推动机器人学以及其它相关学科的发展。它是智能机器人理论和技术的集中体现，能够带动许多相关学科和技术的交叉发展和进步。因此，双足步行机器人的研制具有十分重大的价值和意义。目前，双足机器人设计的目的以及应用的领域主要有，在仿生工程学中，为残疾人、下肢瘫患者或截肢者提供室内和户外的行走工具，利用人工假腿尽可能的使残疾人恢复正常行走功能。其次，在极限环境下代替人工操作，像太空星球表面考察、海底探险、水下资源的开发和设备维修、沉船的寻找和打捞、代替浮游式机器人作为运载工具，以减少推选器对水底的扰动，提高能见度，核电站的监视和维护作业等等各个危险工作。再者，在教育、艺术和大众服务行业都有潜在的应用空间，使双足步行机器人逐步走向大众。1.2国内外研究现状1.2.1国外研究状况最早在1968年，英国的Mosher试制了一台名为“Rig”的操纵型双足1步行机器人，揭开了双足机器人的研究序幕。该机器人只有踝和髋两个关节，操纵者靠力回馈来保持机器人平衡。同期间，南斯拉夫的Vukobratovic提出了一种重要的研究双足机器人的的理论方法，并研制出全世界第一台真正的双足机器人。双足机器人的研制成功促进了康复机器人的研制。法国Poitiers大学力学实验室和国立信息与自动化研究所INRIA机构共同开发了一种具有15个自由度的双足步行机器人BIP2000，其目的是建立一整套具有适应未知条件行走的双足机器人系统。他们采用了分层递解控制结构，使其双足机器人实现站立、行走、爬坡和上下楼梯等。1.2.2国内研究状况国内双足机器人的研制工作起步较晚。1985年以来，相继有几所高校进行了相关的研究并取得了一定的成果。值得一提的是，北京理工大学研制成功我国首例拟人机器人BRH-01。该机器人身高1.58米，体重76公斤，具有32个自由度，每小时能够行走1公里，步幅0.33米。除了能打太极拳，这个机器人还会腾空行走，并根据自身的平衡状态和地面高度变化，实现未知路面的稳定行走。它在系统集成、步态规划和控制系统等方面实现了重大突破，标志着我国双足机器人研究已经跨入世界先进行列。国内其它院校如哈尔滨工业大学、上海交通大学、北京航空航天大学等高等院校也在近几年投入了相当的人力物力进行研制工作。目前，日本和美国对双足步行机器人的研究已经达到了相当高的水平，研制出了能静态或动态行走的多种样机。国内由于起步较晚，与国际最高水平还有一段差距，需要迎头赶上其它国家。1.3双足步行机器人的腿部结构难度最大是模仿人类双足行走的仿人机器人，无论从控制还是配合都十分困难。因为当我们的双足在运动时，总是在单脚支撑，双脚支撑及双脚腾空的几个状态中。对应的驱动方式有完全约束，过度约束，欠约束状态。总而言之，腿部结构是最需要重点设计和研究的。当然，我们的腿部运动十分复杂，为了方便研究我们将模型简化为有六个2自由度的腿部结构，六个自由度完全满足直线行走的要求。由于很多的设计者都采用简单的转动副来完成腿部关节的结构。这样驱动装置的选择就简单多了，最常用的驱动装置有伺服电机，步进电机，谐波减速电机等，这些驱动器最大的优势在于控制量较少，与控制方面比较容易匹配。更进一步的，在仿照人类腿部结构的基础上，如何设计出最佳的腿部比例也要在试验中来获得。1.4本论文研究的主要内容本论文主要研究双足机器人腿部模型的自由多配置，驱动装置的选择，运动模型的建立，以及腿部机构基本尺寸的选择。本论文分为六章：第一章：前言，介绍了双足机器人的国内外发展形势，提出研究的内容和方向。第二章：确定运动方式方法及自由度配置，驱动方式的合理选择及各个零件的尺寸大小和运动范围大小的选择。第三章：研究机器人学的基本理论，包括坐标系的建立，变换矩阵的确定，旋转矩阵的指数，雅克比矩阵的定义及意义。第四章：为双足机器人建立腿部的数学模型，用MATLAB编程相关计算，正逆运动学，机器人关节角度的计算。分析其运动状态，判断运动的可行性。第五章：进行双足机器人的腿部三维模型建立，在Pro/E中设计腿部结构，然后输入相关数据进行运动仿真。第六章：全文总结，对未来的设计做出展望。1.5小结本章中全面的分析了双足步行机器人当前的发展现状和未来的发展趋势，总结了当前研究成绩和方法理论，阐明本论文的研究目的和提出的本论文研究趋势和理论方法。3第二章：双足机器人的模型建立2.1双足机器人的腿部自由度根据当前的研究发现，我们人类腿部有多达76个自由度来供我们完成灵活自由的活动，所以，更多的自由度可以使机器人在运动中具有更高的灵活性，但这也增加了驱动与控制的难度。以此，在腿部自由度的设计中，只能配置较少自由度的情况下尽量模拟人类腿部运动。根据自由度的配置，需要髋、踝、膝关节的共同作用来完成平稳步行。三个关节的主要作用，髋关节：保证上部身体的位姿，保持上半身的稳定，步行中辅助平衡调整步幅。膝关节：调整重心高度以及调整小腿的摆动高度，以适应相应路况。踝关节：起到支撑作用，调整摆动腿与地面的接触，使其接触平稳。基于分析，结合驱动与控制的情况，以及运动的可行性，将腿部自由度简化为6个，首先满足机器人的前进运动，髋、膝、踝各一个自由度，侧向的摆动运动由髋踝关节各布置一个自由度，为了实现转动，在髋关节再布置一个自由度。综上：一共需要配置12个自由度。表格2-1自由度分配表2.2机器人腿部的驱动方式腿部结构的运动方式为关节处的转动。各个关节通过转动副来完成连接。通过转动副便于数据的计算，数据全为角度。单一的运动副也使得机器人的模型得以简化，机器人关节的驱动装置就是机电一体化系统中的执行装置，较为4液压执行装置和气动执行装置，电动执行装置可以获得较大扭矩和功率，而且速度和精度控制较容易实现。对于角度的大小与速度的快慢控制非常严谨，所以选择微型伺服马达。3、微型伺服马达的参数转速：无负载时转过60的时间，普通的速度一般在0.11s/600.21s/60。扭矩：单位为kgcm。对于微型伺服马达是指在舵盘上距离输出轴轴心水平距离1cm处，微型伺服马达能够拉动物体的重量。电压：工作电压一般为4.8V或6V重量：以克为单位，微型9g，中型45g100g等。2.3腿部各个部位的尺寸与运动范围双足步行机器人腿部的设计需要模仿人类实际尺寸和运动范围。人机工程学是研究人与机器及其工作环境之间的相互作用的学科。最基础的便是人体测量学，对人体各部分肢体尺寸做了详细研究和分析，从而对双足步行机器人的仿人的各构件的长度及运动范围的设计提供有力的理论支持。人体尺寸一般是指人体在空间中占用的状态，主要包括高度、宽度和厚度，这些资料根据测量和统计的方法得出。2.3.1人体测量通过参数的测量来研究人体肢体外观形态特征包括高度、长度、厚度及活动范围。人体测量基准，采用严格规定的站姿或坐姿，人体测量的基本体型也必须采用标准型，才能达到数据要求的严谨性，身体均匀，骨骼硬朗，强而有力，肌肉发达，丰满结实，不肥胖。这是人体测量的基本要求。5图2-1人体的基准面和基准轴2.3.2静态人体测量及肢体尺寸确定静态尺寸研究是研究人体肢体长度的问题。我国早在1989年实施的中国成年人人体尺寸中，选取对机器人下肢尺寸有用的数据。在男性人体第50百分位数的人体数据中选择参考数据：身高：1678mm，大腿长度：465mm，小腿长：369mm，足宽：96mm，胫骨点高：444mm，臀宽：306mm，坐姿大腿厚：130mm，坐深：457mm，臀膝距：554mm，最终确定设计都使用比例数据来完成确定，为了便于确定数据首先选择身高作为数据基准。双足步行仿人机器人的腿部结构设定尺寸大小时，选择步行机器人的身高为1000mm，按照合适的比例，然后计算各部分下肢的长度，具体取值如下图表格2-2机器人的主要设计尺寸62.3.3动态人体测量及运动范围确定人类大部分时间处于运动状态，每一个运动时刻我们的肢体总在不同的姿势时伸展运动来达到活动范围。对于步行机器人来说，正是因为这些角度范围的限制，反而有利于计算运动学逆解的时候排除不合实际的情况。根据已经查阅的资料基本确定了人体腿部各关节活动的角度范围以及初步确定双足步行机器人的各关节角度变化范围。表格2-3人体腿部关节角度范围根据实际各个关节的摆动角度范围，拟设计规定了机器人腿部关节的运动范围表格2-4腿部关节角度变化范围2.4小结根据双足步行机器人最少自由度和驱动装置的选择，按照人体真实测量数据，为双足步行机器人下肢结构的实体建立模型确定了尺寸的大小和各个关节角的角度范围。7第三章机器人学的基本相关理论3.1坐标变换为了控制双足机器人各个肢体的活动位置，我们必要的定义一个空间中的全局坐标系来确定各个部件在空间中的相对位置。为了便于参考，坐标系的初原点设在机器人处于正常站立时重心的正下方，然后相对机器人的视野来定义，X轴指向正前方，Y轴指向左侧，Z轴指向正上方。如下：图3-1机器人世界坐标系3.1.1局部坐标系和齐次变换通过髋部关节转动而引起的双足位置的变化来研究其对应位置关系。在图3.2(a)中，机器人左腿在世界坐标系中的位置为，因而髋部到脚踝部的位置Pa用矢量来表示。从图中可以得出：r=+式3-1Par右腿站立，左腿摆动时，如图3.2(b)位置，腿部伸出状态下髋部到脚踝的矢量用表示，这时脚踝处可以表示为：r=+式3-2Phar以上两个状态是在髋部位置固定不变的情况下，矢量随腿部转动到Par矢量。r8图3-2世界坐标系和左腿局部坐标系根据图3.2中与腿固定的坐标系就被称作为局部坐标系。其与大地a固定的坐标系不同的是，局部坐标系是随着机器人大腿小腿构成的连杆的位姿变化的，是一个可以运动的“动坐标系”。坐标系是由以髋部作为原点和表示x、y和z轴的单位向量组成的。局部坐标系与转角的数学关系就可以表示成：Eax=Eay=Eaz=式3-301sinco0cosin0由于摆动腿的局部坐标系是绕x轴转的，所以只有Eay和Eaz发生改变。所以这三个矢量可以用一个33矩阵表示出来，具体矩阵如下：Ra=EaxEayEay式3-4通过上面的矩阵，左腿在运动和静止时的矢量关系如下：=Ra式3-5r由上面的公式我们可以得出结论，矩阵和原矢量的叉乘可以表示原矢量转动后得到的矢量。为了研究位置信息，定义左脚脚踝处在局部坐标系中的a位置为。其中左上角标表示的位置点是相对的坐标系，相对的坐标系为世界Ph坐标系时左上角标省略。于是，从图3.2(a)中我们可以得到：=式3-6Phr在图3.2运动过程中，左腿局部坐标系与左腿一起摆动，因而固定不变。Ph所以摆动腿端部（脚踝处）的位置可用两种方式来描述：在世界坐标系中表示脚踝的位置为wPh在左腿局部坐标系中表示脚踝的位置为a9而这两种表示方式之间的关系可有式3-2、式3-5和式3-6联立得出：=+Ra式3-7Pha式3-7用矩阵的方式表示出来为：=式3-8110PRh从公式可以得出，为了使矩阵运算准确，在转动矩阵和位置矢量组成矩阵后须添加0和1，凑成为44矩阵完成计算。于是我们定义这样的44矩阵：Ta=式3-910PaR这个矩阵Ta称为齐次变换矩阵，矩阵中是旋转矩阵和位置矢量有规律的添加0和1构成的。于是局部坐标系描述变换到世界坐标系描述就可以表示为：=Ta式3-10P3.1.2齐次变换的链乘法则为了建立左腿髋关节处与膝关节处两个局部自由度之间的关系，我们现在假想的将世界坐标系移动到左腿髋关节处，这样膝关节坐标系就被称为髋关节坐标系的局部坐标系，这样坐标系分别分配为：和ab坐标系中观察脚踝的位置到坐标系中观察脚踝的位置的bPhPh矩阵变换与3.1.2节中的变换相似，可以表示为：=Tb式3-111Ph1Ph这时的齐次变换矩阵Tb定义如下：Tb=式3-120Ra其中是在局部坐标系中描述局部坐标系的原点位置。Pbab进而将式3-11代入式3-8中可以得到：=TaTb式3-131h1h式3-13表示了，从膝关节的局部坐标系中描述脚踝位置到从世界坐标10系中描述脚踝位置的变换。其中可以公式中右侧的两个齐次变换矩阵的乘积构成了一个新的齐次变换矩阵。Tb=TaTb式3-14对于以上分析，我们可以做迭代处理而得出更加有用的结论。假设有一个机械链条把到局部坐标系连接起来，而且定义相邻两个坐标系之间1N和的齐次变换矩阵为Ti+1，那么迭代以上讨论就可以得出：iTn=T1T2T3Tn式3-15公式中Tn为世界坐标系中表示第N个关节的位姿的齐次变换矩阵。如果需要追加关节只需将对应的齐次变换矩阵右乘到之前的矩阵之上即可。所以将齐次矩阵依次右乘进行坐标变换的计算规则称为链乘法则。3.2转动特性3.2.1侧摆、俯仰和转动空间坐标系中，转动特性是通过绕坐标轴的旋转运动实现的，本说明书中使用的用RPY组合变换表示运动姿态。RPY组合是一种常用的旋转集合，它包含了侧摆（roll）、俯仰（pitch）和转动（yaw）。具体规定绕x轴旋转称为侧摆；绕y轴旋转称为俯仰；绕z轴旋转称为转动。如图所示分别展示了侧摆、俯仰和转动的情况。图3-3侧摆、俯仰和转动情况下表中定义了RPY组合对应的转动轴、名称和转动角的代表符号。对应的侧摆、俯仰和转动的旋转矩阵依次为：11表3-1RPY组合说明Rx（）=cosin001Ry（=)s01Rz()=10coin机械链的运动姿态往往由一个绕轴的旋转序列来规定。在任何旋转序列系，旋转次序都是尤为重要的。用公式作如下规定：Rrpy()=Rz（Ry()Rz(,)=10cosincos0sin1cosin001=CSCSSC其中C=cos，S=sin，其余类似。3.2.2旋转矩阵的说明旋转矩阵在应用中存在两种含义。一种含义是在一个已知的坐标系中对其中矢量的转动作描述，研究对象是矢量。当然这个矢量的转动是有限制的，将这个矢量看作是一个射线的话，这个射线的端点与它运动所在的坐标系原点重合，而射线方向的改变就可以用旋转矩阵来描述。12图3-4矢量转动描述另一种含义是表示局部坐标系的姿态改变的描述。在坐标系中定义一个固定点，在坐标系姿态转换后重新在新的坐标系中描述这个定点时用到旋转矩阵，这时研究对象是一个固定点。当然坐标系姿态的转动也是有限的，就是规定姿态变换前后两个坐标系的原点是重合的。通俗的说就是换一个视角去观察同一个物体。图3-4对坐标轴转动描述这两种描述方式看似相近，但其实有很多区别点：1、研究的对象不同，第一种含义研究的是矢量，第二种含义研究的是固定点。2、研究对象的运动状态不同，第一种含义中的矢量是运动的，第二种含义中的点是固定的。3、研究中坐标系的个数不同，第一种含义中在同一个坐标系中研究，第二种含义中产生了新的坐标系。3.2.3旋转矩阵的性质下面来讨论旋转矩阵的一个重要性质：旋转矩阵是正交矩阵。假设讨论一个描述局部坐标系姿态的旋转矩阵，在这个33矩阵中，是由单位矢量ex，ey，ez构成的。这三个矢量是两两相互垂直的，故他们正交：Eiej=式3-17ji0113用矩阵的形式重新表示时：RR=Eezyxezezyxx10所以经过以上推算，旋转矩阵的逆矩阵等于其转置。从而证明了旋转矩阵是正交矩阵。3.2.4角速度矢量在三维空间中表示转动速度时，可以表示成为绕某个轴线的转动。如图3.6，是绕坐标轴z轴做的转动。图3-5绕z轴转动三维物体以上也用矢量的形式表示：=式3-20w10这样就定义了角速度矢量，其中每个元素的单位都是rad/s。之后在研究机器人运动时，其运动关节处的驱动形式就是以电机转速表示的，故研究转速作为机器人基础量就尤为重要。由式3-20可以发现，当转动轴线定为坐标轴时，就可以将角速度矢量做归一化处理，将速度矢量分解为一个单位速度矢量和转速标量的乘积。=q（表示绕坐标轴旋转的单位角速度矢量）式3-21wa为了研究某点的速度，如关节给定角速度的情况下肢体某处的运动速度，就需要进一步研究角速度与速度的关系。在转动的肢体上选择一个研究点，并用矢量的方式表示该点，如图3.7，而这一点的速度就可以用角速度矢量与该点矢量的叉积来表示。14图3-6速度的表示=其速度方向由叉积中的右手定则来规定，即弯曲右手四指，四指vwp由沿着小于180的方向转，这时拇指指向的方向就是速度的方向。并且角速度矢量本身也能够转动，在式3-21两边同时左乘旋转矩阵R得：R=Rq=R,=R，=q式3-23wawa由以上推导可以得出，角速度矢量的旋转可以直接用旋转矩阵进行变换。当角速度矢量、位置矢量和速度矢量在旋转矩阵的作用下转动时有：=R，=R，=Rwpv由叉积定义可知，转动前后的关系为：=vw所以满足：R（）=（R）（R）式3-24pp3.2.5旋转矩阵的微分与角速度矢量物体上的一点旋转后局部坐标系与世界坐标系之间的关系为：=式3-pR25上式对时间微分可得该点在世界坐标系中的速度，但由于局部坐标系中的点相对局部坐标系没有运动，所以不存在微分关系，故：=式3-26pR将2-23两边左乘:=Rt式3-27p将3-25代入3-24得：=RR式3-28上公式描述了用局部坐标系中的点的位置来表示世界坐标系中该点的速度的计算过程。联系世界坐标系中某点速度的计算，式3-22，本节中某点相对时间的微分表示的含义就是这一点的速度，即=，故：=式3-29pvpw应用叉积计算公式可得：15=式3-30wp00WzyxPzy用矩阵相乘形式重新编写速度矢量可得：=式3-31wp00WzyxPzySp所得的新矩阵S，也有很多良好的性质。所以为了表示方便现在需要定义一种新的计算。这个新的矩阵S，通过观察可以发现它在转置后只是改变了符号：St=S在数学上这样的矩阵被称为反对称矩阵。下面来为了计算方便来定义一种运算，由三维矢量导出对应的反对称矩阵要操作符“”表示，由反对称矩阵导出对应的三维矢量用操作符“”表示。如下：=wWzyx00Wxyz=式3-320yzyw于是角速度矢量在计算时就可以使用以下表示方式：=式3-33wp为了表示方便，可以在表示三维角速度矢量导出对应反对称矩阵时改写为，w而且旋转矩阵与角速度矢量的关系为：=RRtw3.2.6矩阵指数为了得到旋转矩阵与角速度矢量之间的关系，作如下分析。在公式3-34两边同时右乘R可得：=R式3-35w规定角度的方向时，是以角速度矢量垂直平面为基准平面，并按照右手法则确定的。先定义三维空间内矢量分解，在三维空间中给定任意矢量v做正交分解，其中一个分量沿单位矢量的方向，并称为轴向分量，记为V；另一个分量沿着矢量垂直方向，并称为法向分量，记为V。如图3.8所示有v16=V=-=(E-)式3-36Vtvvv图3-7某点p绕定轴转并且定点运动的刚体的任何位移，均可以以通过该定点的一个轴的旋转运动来表示，这是理论力学中的欧拉定理阐述的。图3.8(a)就表示了这一旋转运动，转轴A通过定点O，转轴所对应的角速度矢量为。刚体上的一点P所对应的位置矢量为，经过角度的旋转，转动至p点（位置矢量为p）。旋p转所在平面交旋转轴于Q点。按照矢量关系有p=Q+P式3-37O其中，将做正交分解，就为方向的轴向分量，根据式3-36有：pQ=式3-38QP就为方向的法向分量，根据式3-36有：P=(E-)式3-39Qp如图3.8(b)在刚体旋转平面上可得：P=(cos)P+（sin）P式3-40QQ在三维坐标中向量P同时垂直于和p两个矢量，故可用和p的叉积来表示：P=式3-41p将以上联立得:P=cos(E-r)+sin式3-42Q3.3雅克比矩阵雅克比矩阵描述了关节微小位移与空间运动之间的关系。由机器人雅克比矩阵可以推导出关节力矩、机器人与外部外力之间的关系。在机器人运动学和动力学中都有广泛的应用。本节利用机器人逆运动学来推导雅克比矩阵。如图3.9所示为机器人单条腿的腿部结构，从原点到端点分别定义连杆为1到N。并且在正运动学已经计算完成的情况下完成后续的推导。17图3-9雅克比矩阵计算假设在如图3.9的腿部关节中，只有第二个关节转动微小角度，引起第N杆的微小位移和微小转动可得到以下关系：n=w（n-2）q2paPn=wq2式3-43W根据以上推导，为每一个杆件都做计算，之后将各个杆件对N杆的运动影响叠加起来，就是N杆最终的运动状态：N=Pnjp1N=式3-44Wwnj13.4本章小结本章内容是在阐述机器人相关数学基础，其中推导了变换矩阵、旋转矩阵的矩阵指数运算、定义了转动运动、雅克比矩阵等，为后续数学建模提供了理论基础。18第四章双足机器人建模4.1双足机器人数据结构的建立4.1.1数据选择双足机器人的本体结构可以理解为是由很多杆件和关节连接构成的，最重要的问题就是简化数据编写和计算，进一步对机器人进行有效的分解，是本章研究的要点。步行机器人的数据库是建立在很多局部坐标系与世界坐标系之间点、速度、角速度和力、力矩等量的关系。它的肢体可以抽象为杆件，联动运动的部分是连接杆件的关节部分，为了建立合理的数据结构先将机器人做如下分解。图4-1机器人腿部结构的分解每一个杆件都包含一个关节。编程时，将每一个结构定义为对应的名称。各个结构之间的关系采用二叉树数据模式构成，构成原则是：本级别的分解部分作为二叉树的根结点，下一级别的分解部分作为二叉树的左子树（可以看作家庭关系中的“子”），同一级别的最近的分解部分作为二叉树的右子树（可以看作家庭关系中的“兄弟”），具体分配图如图：图4-2机器人腿部结构分解19为了将上述分解变成实际数据结构，必须给每个分解部分对应标号并建立数据表4-1数据列表这些数据可以在MATLAB程序中直接输入：图4-3结构体变量输入4.1.2数据的遍历在递归算法使用时，必须有一个明确的递归结束条件，又被称为递归出口。其实这种方式也对应二叉树的遍历方式，以下用前序遍历法编写显示各个连杆名称的函数。图4-4递归算法的编写函数在每次调用自己的时候就会在结构中向下移动一层，直到无姐妹和子的结点。这样如图4.2的树形结构就可以完成整个结构树结点的访问计算，并且不会无穷循环，提供了可靠的递归出口。同时也是得编程语句变得极为精简，而且便于理解。4.2数学模型的构造构造数学模型是为步行机器人对应它的分解部分给他们赋予标号，然后为每一个分解部分构造一个拥有大量数据的结构体变量，再构造结构体数组来完成20整个机器人数字模型的建立。下面就一一介绍结构体数组中的各个变量的定义和分配。首先需要对有12自由度的仿人机器人腿部的连杆进行编号，每个自由度驱动装置必须满足运动：图4-5关节标号分配各个局部各个局部坐标系之间的相对位置关系是由关节轴矢量和相对位ai置矢量来确定，如下图，结构体中添加对应变量：关节轴矢量“a”，bi其中，关节轴矢量是指相对于母连杆转动的单位角速度矢量，其旋转方向是由右手法则来定义，膝关节关节轴矢量为：5=11=a01相对位置表示的是一个局部坐标系的原点在其母连杆坐标系中的位bi置。如踝关节处，R7和R13分别与其母连杆R6和R12所在的局部坐标系原点重合，故：7=15=b021图4-6关节参数设定连杆关节角、关节速度、杆件质量等一些参数的设定，具体设定见表表4-2连杆结构体变量表4.3正运动学正运动学可以用于机器人重心计算、机器人可视化位姿计算和机器人与环境位置方位关系等计算。4.3.1三维矢量与反对称矩阵22生成的反对称矩阵是在原有轴矢量的数据基础之上生成的特殊形式，其操作符在第三章3.2.5节中已经定义，对应的变换定义式为式3-32。根据以上定义分别编写函数a_0与函数a_1来完成操作符转换的任务。其中函数a_0完成操作符“”的定义，函数a_1完成操作符“”的定义。图4-7函数完成操作符需要注意的是，这两个函数都同时具有传送和返回数据的作用。调用对应函数时需注意对应的传递值和返回值的形式，如函数a_0中传递值为三维矢量，返回值为反对称矩阵，函数a_1反之。4.3.2矩阵指数计算旋转矩阵R=E+sinq+2(1-cosq)式4-1a根据式4-1可以编写对应函数Rodrigues，如下：图4-8旋转矩阵计算函数函数Rodrigues中的传递输入值为两个数据分别为关节轴矢量和关节角。关节轴矢量为三维单位矢量，是根据机器人关节的转动方向已经完成规定的，对应关节数据已经分配在LINK结构体数组中。关节角表示对应关节弯曲的角度值，数据以弧度制表示。4.3.3正运动学计算正运动学如下图所示，为单个连杆的齐次变换矩阵的推导。首先将连杆的局部坐标系的原点设定到转轴上，这样在其母连杆坐标系中，关节轴矢量为i，相对位置矢量（局部坐标系原点在母连杆坐标系中的位置）为。abi23图4-9单个连杆位姿连杆相对于母杆的齐次变换矩阵为：iTj=式4-210bjqae之后研究如图4.10两连杆连接情况。假定母连杆的绝对位置和姿态PiR，那么母杆的齐次变换矩阵（相对世界坐标）就为：iT=式4-310PiR这样，连杆的齐次变换矩阵（相对世界坐标）可由链式法则得到：iTj=TTj式4-4j联立式4-1、式4-2和式4-3可知，对应连杆的位置和姿态可以分别求i出：=+Rbi式4-5PjiRj=Rieaj式4-6a以上程序在主程序中调用之前，需要对BODY连杆的位姿首先予以确定，也就是给LINK(1).p和LINK(1).R确定的值以确定BODY的绝对位姿。然后给出各个关节的关节角（LINK(*).q），之后在主程序中运行ForwardKinematics(1)就能完成全部杆件的位姿计算。4.4逆运动学运动学可以用于给定机器人腿部和身体姿态后求取对应关节角度的计算。就是在给定端部杆件的位姿之后求出全部关节角度，可以看作正运动学的逆运算。244.4.1解析法求解逆运动学先分析双足步行机器人模型的右下肢，在计算准备时，必须让上半身（1R1）和左下肢（7R7）定义为静止。然后定义从身体坐标系原点pP到髋关节距离为D，大腿长度为A，小腿长度为B，详细图示如下：2=1+R式4-7P0图4-10腿部关节逆运动计算在足部踝关节的局部坐标系中，髋关节的位置为：=R7（p2-p7）=式4-8rRyx如上图膝关节，故C可以求出：C=式4-922zRyx这样在三角形ABC中，运用余弦定理有：C=A+B-2ABcos（）式4-1022q根据式4-10可导出膝关节角度：q=-cos+式4-111ABC22由正玄定理：=式4-12)sin(qaAsin由此a=-sin式4-131CA根据矢量分解，踝关节角q=atan2（Ry，Rz）q=-atan2Rx，sign（Rz）-a2Rzysign（）函数，含义为取符号函数，即当x为正时返回+1，当x为负时25返回-1，当x为零时返回0。之后根据已经求出的角度值代入连杆姿态关系式中：R7=R1Rz（q2）Rx（q3）Ry（q4）Ry(q5+q6)Rx(q7)式4-14根据式4-9推导可得：Rz（q2）Rx（q5）Ry（q4）=R1R7Rx（q7）Ry(q5+q6)式4-15式4-15左右两边分别展开和计算有：=式4-1643345222CSCSCS321R根据4-16矩阵得：q=atan2(-R,R)212q=atan2(R,-RS+RC)432q=atan2（-R，R）5134.4.2雅克比矩阵以3.3节中雅克比方程的数学推导作为基础，运用MATLAB建立方程，完成雅克比矩阵的计算。根据式3-59、式3-62完成雅克比矩阵函数Jacobian的编写。4.5测试程序这里的测试使用了GUI图形交互功能，它是MATLAB中与用户的一种交互方式。关于测试正逆运动学推导程序的GUI程序，如下图所示26图4-11正逆运动学测试界面其中，程序中对应计算的是机器人右腿的正逆运动学参数。程序界面中分为正运动学计算和逆运动学计算。正运动学计算中首先需要输入已知参数：主体位姿和六个关节的转动角度。其中主体位姿需要分别输入旋转矩阵和位置矩阵，输入完成后点击“开始计算”就能计算出脚踝第六个自由度对应的位姿状态，并且显示在界面中。4.5.1模拟步行数据计算与参数分析预先给定模拟步行的约束条件，在模拟机器人步行运动时，确定步行过程中两脚之间最大的距离为：l；BODY高度为：h。计算生成从准备步行、步行一个完整周期、最后完成步行并站立，预定时长为60秒。根据以上设定，完成程序编写。首先，下蹲保持弯腿站立姿势，其目的有两个方面：1、能够为之后行走计算时的数值法逆运算摆脱奇异姿态的初始状态（竖直站立时，为一种奇异状态，此时数值法中雅克比矩阵行列式为0，因而不存在雅克比逆矩阵）。2、是为了在行走过程中为了保持一定的高度，需要双腿弯曲以保证双腿行走时的迈步范围。具体执行函数为step_start_final。最后，为初始数据修改一些内容，并将计算结果的弧度值，全部转换为角度值。并使用函数shujushengcheng完成待导入Pro/E中的机械表格数据的生成任务。以上详细主函数流程图见下图(a)。27图4-12程序流程图下面分述以上用到的主要函数：1、step_start_final函数：首先，运用解析法计算机器人竖直站立的奇异位姿与非奇异位姿（即双腿微微弯曲的站立状态）之间的转换。在循环过程中将计算出来的数据记录到全局变量times和datas中。输入数据为：hqa表示目的上体(BODY)高度与绕z轴角度，t表示执行时间长度，输出数据2、step_new函数：程序总思想与step_start函数一致，只是在逆运动学求解时使用的方法为数值法。对于aba的设定具体按照人体在步行时的四种状态区分的，这四种状态分别是：1、运动过程中左脚在前，并且运动趋势为两脚在迈步的状态（两脚远离身体中心轴线）；2、运动过程中左脚在前，并且运动趋势为两脚为并步状态（两脚向身体中心轴线靠近）；3、运动过程中右脚在前，并且运动趋势为两脚在迈步的状态（两脚远离身体中心轴线）；4、运动过程中右脚在前，并且运动趋势为两脚为并步状态（两脚向身体中心轴线靠近）。并且对应数值如表。在整个运动过程，主要让其完成下三个步骤：1）、由直立状态下蹲到双腿弯曲状态，用step_start_final函数计算；282）、计算走一步（四个周期）的角度值，用step_new函数计算；3）、由双腿弯曲状态转换为直立状态，用step_start_final函数计算。其中，一步中的四个周期为：1）、迈出右脚，对应状态号3；2）、收回左脚，对应状态号4；3）、迈出左脚，对应状态号1；4）、收回右脚，对应状态号2。3、shujushengcheng函数将计算结果分别存储到q1q12变量中，分别对应12个关节的转动角度，并且储存为PROE中能够输入的数据形式。这里来计算上体高度与行走步长之间的关系，并测定各个关节角度范围。其运动关节主要运用了xz平面中的6个自由度完成行走。根据几何关系可以得知，行走过程中双腿处于伸直状态时为理论极限位置，如图所示：图4-13上体直立行走如上图所示，在一定高度时，双脚步行步长过大时，双腿长度就无法满足要求，也就是当一只腿的