六轴运动机器人运动学求解分析_第九讲
机器人运动学和动力学分析及控制
机器人运动学和动力学分析及控制引言随着科技的不断进步，机器人在工业、医疗、军事等领域发挥着越来越重要的作用。
而机器人的运动学和动力学是支撑其运动和控制的重要理论基础。
本文将围绕机器人运动学和动力学的分析及控制展开讨论，探究其原理与应用。
一、机器人运动学分析1. 关节坐标和笛卡尔坐标系机器人运动学主要涉及的两种坐标系为关节坐标系和笛卡尔坐标系。
关节坐标系描述机器人每个关节的转动，而笛卡尔坐标系则描述机器人末端执行器在三维空间中的位置和姿态。
2. 正运动学和逆运动学正运动学问题是指已知机器人每个关节的位置和姿态，求解机器人末端执行器的位置和姿态。
逆运动学问题则是已知机器人末端执行器的位置和姿态，求解机器人每个关节的位置和姿态。
解决机器人正逆运动学问题对于实现精确控制非常重要。
3. DH参数建模DH参数建模是机器人运动学分析中的重要方法。
它基于丹尼尔贝维特-哈特伯格(Denavit-Hartenberg, DH)方法，将机器人的每个关节看作旋转和平移运动的连续组合。
通过矩阵变换，可以得到机器人各个关节之间的位置和姿态关系。
二、机器人动力学分析1. 动力学基本理论机器人动力学研究的是机器人在力、力矩作用下的运动学规律。
通过牛顿-欧拉方法或拉格朗日方程，可以建立机器人的动力学模型。
动力学模型包括质量、惯性、重力、摩擦等因素的综合考虑，能够描述机器人在力学环境中的行为。
2. 关节力和末端力机器人动力学分析中的重要问题之一是求解机器人各个关节的力。
关节力是指作用在机器人各个关节上的力和力矩，它对于机器人的稳定性和安全性具有重要意义。
另一个重要问题是求解末端执行器的力，这关系到机器人在任务执行过程中是否能够对外界环境施加合适的力。
3. 动力学参数辨识为了建立精确的机器人动力学模型，需要准确测量机器人的动力学参数。
动力学参数包括质量、惯性、摩擦等因素。
动力学参数辨识是通过实验方法，对机器人的动力学参数进行测量和估计的过程。
机器人运动学与动力学分析
机器人运动学与动力学分析引言：机器人技术是当今世界的热门话题之一。
从生产领域到服务领域，机器人的应用越来越广泛。
而要实现机器人的精确控制和高效运动，机器人运动学与动力学分析是必不可少的基础工作。
本文将介绍机器人运动学与动力学分析的概念、方法和应用，并探讨其在现代机器人技术中的重要性。
一、机器人运动学分析机器人运动学分析是研究机器人运动的位置、速度和加速度等基本特性的过程。
运动学分析主要考虑的是机器人的几何特征和相对运动关系，旨在通过建立数学模型来描述机器人的运动路径和姿态。
运动学分析通常可以分为正逆解两个方面。
1. 正解正解是指根据机器人关节位置和机构参数等已知信息，计算出机器人末端执行器的位置和姿态。
正解问题可以通过利用坐标变换和关节运动学链式法则来求解。
一般而言，机器人的正解问题是一个多解问题，因为机器人通常有多个位置和姿态可以实现。
2. 逆解逆解是指根据机器人末端执行器的位置和姿态，计算出机器人关节位置和机构参数等未知信息。
逆解问题通常比正解问题更为复杂，因为存在多个解或者无解的情况。
解决逆解问题可以采用迭代法、几何法或者数值优化方法。
二、机器人动力学分析机器人动力学分析是研究机器人运动的力学特性和运动控制的基本原理的过程。
动力学分析主要考虑机器人的力学平衡、力学约束和运动方程等问题，旨在实现机器人的动态建模和控制。
1. 动态建模动态建模是研究机器人在外力作用下的力学平衡和运动约束的数学描述。
通过建立机器人的运动方程，可以分析机器人的惯性特性、静力学特性和动力学特性。
机器人的动态建模是复杂的，需要考虑关节惯性、关节力矩、摩擦因素等多个因素。
2. 控制策略机器人动力学分析的另一个重要应用是运动控制。
根据机器人的动态模型，可以设计控制策略来实现机器人的精确运动。
常见的控制方法包括PID控制、模糊控制、自适应控制等。
通过合理选择控制策略和调节参数，可以实现机器人的平滑运动和高精度定位。
三、机器人运动学与动力学分析的应用机器人运动学与动力学分析在现代机器人技术中具有重要的应用价值。
求解机器人运动学方程的步骤
求解机器人运动学方程的步骤嘿，咱今儿个就来说说求解机器人运动学方程的那些事儿！你想想啊，机器人就像个超级厉害的小机灵鬼，要让它乖乖听话，就得搞清楚它咋运动的呀！
第一步呢，那可得先把机器人的结构给摸透咯！就好比你要了解一个新朋友，得知道他的胳膊腿儿都长啥样，怎么活动的。
这一步可不能马虎，得仔细研究机器人的各个关节啊、连杆啊啥的。
第二步呢，就像是给机器人搭积木一样，得把那些关节和连杆之间的关系给理清楚。
这就像是搭房子，得把砖头一块块稳稳地放好，才能盖出结实的房子来。
第三步呀，咱就得用一些数学魔法啦！什么三角函数啦、矩阵啦，都得派上用场。
这就好像是给机器人施了个魔法咒语，让它能按照咱的想法动起来。
第四步呢，可别小瞧了，得反复验证。
就跟你考试做完题得检查一样，看看算得对不对，机器人能不能真的按照咱想的那样动。
你说这求解机器人运动学方程难不难？当然难啦！但咱要是把每一步都走踏实了，就像走楼梯一样，一步一个脚印，那也能攻克这个难关呀！
你再想想，要是咱能轻松搞定机器人的运动学方程，那多牛啊！就好像咱掌握了机器人的小秘密，能指挥它干啥就干啥。
这感觉，是不是超棒？
而且啊，这就跟解谜题一样，一点点地解开，最后找到答案，那成就感，简直爆棚！你说是不是？咱可不能被这小小的方程给难住咯，得鼓起劲儿来，跟它斗一斗！
所以啊，求解机器人运动学方程虽然有挑战，但咱别怕呀！只要咱用心去研究，去琢磨，肯定能搞定它！加油吧，小伙伴们！让我们一起征服机器人运动学方程这个小怪兽！。
机器人 运动学
机器人运动学机器人运动学机器人运动学是研究机器人运动规律和运动控制的学科。
它是机器人技术的重要组成部分，对于机器人的设计、控制和应用具有重要意义。
机器人运动学主要研究机器人在空间中的运动规律，包括位置、速度和加速度等。
通过研究机器人的运动学特性，可以实现对机器人的精确控制和规划。
机器人运动学主要包括正运动学和逆运动学两个方面。
正运动学是指根据机器人关节的位置和长度，求解机器人末端执行器的位置。
它通过解析几何、向量运算和矩阵变换等数学方法，将机器人关节的位置参数转化为末端执行器的位置参数，从而实现对机器人的位置控制。
逆运动学是指根据机器人末端执行器的位置，求解机器人关节的位置和长度。
逆运动学是机器人运动学的核心内容，也是机器人控制的关键问题之一。
通过逆运动学，可以实现对机器人末端执行器的精确控制，从而实现机器人在空间中的精确定位和定向。
机器人运动学的研究还包括机器人的姿态和轨迹规划。
姿态是指机器人在空间中的朝向和姿势，轨迹是指机器人在运动过程中的路径和速度。
通过研究机器人的姿态和轨迹规划，可以实现机器人在复杂环境中的灵活运动和避障控制。
机器人运动学的应用非常广泛。
在工业领域，机器人运动学被应用于自动化生产线的控制和优化，实现了生产效率的提高和生产成本的降低。
在医疗领域，机器人运动学被应用于手术机器人的控制和操作，实现了微创手术和精确手术的目标。
在军事领域，机器人运动学被应用于无人飞机和无人车辆的控制和导航，实现了作战效能的提高和战场风险的降低。
机器人运动学的发展离不开先进的传感器和控制技术的支持。
传感器可以实时感知机器人的位置和环境信息，控制技术可以根据机器人的位置和运动规律，实现对机器人的精确控制和运动规划。
总结起来，机器人运动学是研究机器人运动规律和运动控制的学科，主要包括正运动学、逆运动学、姿态和轨迹规划等内容。
机器人运动学的研究和应用对于机器人技术的发展和应用具有重要意义，将为我们创造更多的便利和机会。
机器人运动学
机器人运动学机器人运动学是研究机器人运动和姿态变化的一门学科。
它通过分析机器人的构造和动力学参数，研究机器人在特定环境下的运动规律和遵循的动力学约束，以实现机器人的准确控制和运动规划。
本文将从机器人运动学的基本概念、运动学模型、运动学正解和逆解等方面进行介绍。
1. 机器人运动学的基本概念机器人运动学是机器人学中的一个重要分支，主要研究机器人在空间中的运动状态、末端执行器的位置和姿态等基本概念。
其中，运动状态包括位置、方向和速度等；末端执行器的位置和姿态是描述机器人末端执行器在空间中的位置和朝向。
通过研究和分析这些基本概念，可以实现对机器人运动的控制和规划。
2. 运动学模型运动学模型是机器人运动学研究的重要工具，通过建立机器人的运动学模型，可以描述机器人在运动过程中的运动状态和姿态变化。
常见的运动学模型包括平面机器人模型、空间机器人模型、连续关节机器人模型等。
每种模型都有其独特的参数和运动学关系，可以根据实际情况选择合适的模型进行分析和研究。
3. 运动学正解运动学正解是指根据机器人的构造和动力学参数，求解机器人末端执行器的位置和姿态。
具体而言，根据机器人的关节角度、关节长度和连杆长度等参数，可以通过连乘法求解机器人末端执行器的位姿。
运动学正解是机器人运动学中的常见问题，解决这个问题可以帮助我们了解机器人在空间中的运动规律和运动范围。
4. 运动学逆解运动学逆解是指根据机器人末端执行器的位置和姿态，求解机器人的关节角度。
反过来，控制机器人的运动状态就需要求解逆运动学问题。
运动学逆解是机器人运动学研究的重要内容之一，它的解决可以帮助我们实现对机器人的准确定位和控制。
总结：机器人运动学是研究机器人运动和姿态变化的学科，通过运动学模型、运动学正解和运动学逆解等方法，可以描述机器人的运动状态、末端执行器的位置和姿态。
深入研究机器人运动学，可以实现对机器人的准确控制和运动规划。
随着机器人技术的不断发展，机器人运动学的研究也得到了越来越广泛的应用和重视。
举例说明机器人运动学正解的求解过程 -回复
举例说明机器人运动学正解的求解过程-回复机器人运动学正解是指根据机器人的关节坐标和末端执行器坐标来计算机器人的关节变量，以实现特定的末端执行器运动。
在此过程中，通过利用几何学和代数学的知识，可以推导出机器人的正解方程，并将其转化为求解关节变量的问题。
下面将详细介绍机器人运动学正解的具体求解过程。
1. 建立机器人的坐标系：首先，需要确定机器人坐标系的建立方式。
一般来说，机器人坐标系可以分为基座标系（也称为基座标系）和末端执行器坐标系。
基座标系用于描述机器人的位置和朝向，而末端执行器坐标系用于描述机器人末端执行器的位置和朝向。
2. 确定机器人的关节参数：机器人的关节参数包括关节长度、关节角度、关节型号等。
这些参数的确定是根据机器人的实际结构和设计需求来确定的。
3. 建立机器人的正解方程：机器人的正解方程描述了机器人的末端执行器坐标与关节坐标之间的关系。
一般来说，机器人的正解方程可以通过运动学链式法则得到。
链式法则是基于连续的变换矩阵构建的，每个关节均有一系列变换矩阵，最终得到机器人的正解方程。
4. 求解机器人的正解方程：根据机器人的正解方程，我们可以将末端执行器坐标作为已知量，求解关节变量。
这一步可以通过将正解方程转化为一个线性方程组来实现。
一般来说，线性方程组的求解可以通过矩阵运算或数值计算方法来实现。
5. 解的复现和验证：求解得到的关节变量需要进行复现和验证。
这一步可以通过将求解得到的关节变量带入机器人的正解方程中，计算得到新的末端执行器坐标，与原始的末端执行器坐标进行对比，以验证求解结果的准确性。
总结起来，机器人运动学正解的求解过程包括建立机器人的坐标系、确定机器人的关节参数、建立机器人的正解方程、求解机器人的正解方程以及解的复现和验证。
这一过程需要运用几何学、代数学和数值计算等知识，通过推导和计算来实现机器人的正解。
通过机器人运动学正解，我们可以根据给定的末端执行器坐标来计算机器人的关节变量，从而实现特定的末端执行器运动。
机器人运动学正解逆解课件
在机器人力控制中，需要知道每个关节的角度变化来调整 机器人的姿态和力矩。逆解可以用于求解每个关节的角度 变化，从而调整机器人的姿态和力矩。
机器人定位
在机器人定位中，需要知道每个关节的角度变化来调整机 器人的位置和姿态。逆解可以用于求解每个关节的角度变 化，从而调整机器人的位置和姿态。
04
实现复杂运动轨迹
利用运动学正解与逆解，可以规划出 复杂的运动轨迹，满足各种应用需求 。
02
机器人运动学正解
正解的基本概念
正解是指机器人末端执行器从某一初 始位置和姿态到达目标位置和姿态所 需经过的关节角度值。
正解是机器人运动学中的基本问题， 是实现机器人精确控制和自主导航的 基础。
正解的求解方法
逆解的求解方法
01
代数法
通过建立机器人关节角度与目标点坐标之间的方程组，利用数学软件求
解方程组得到关节角度。这种方法适用于简单的机器人结构，但对于复
杂机器人结构求解过程可能较为繁琐。
02
数值法
通过迭代或搜索的方法，不断逼近目标点坐标，最终得到满足要求的关
节角度。这种方法适用于复杂机器人结构，但求解时间较长且可能存在
机器人运动学正解逆解课件
目 录
• 机器人运动学概述 • 机器人运动学正解 • 机器人运动学逆解 • 机器人运动学正逆解的对比与联系 • 机器人运动学正逆解的实例分析
01
机器人运动学概述
定义与分类
定义
机器人运动学是研究机器人末端 执行器位姿与关节变量之间的关 系的学科。
分类
根据机器人的结构和运动特性， 可以分为串联机器人和并联机器 人。
局部最优解。
03
解析法
通过几何学和代数学的方法，直接求解关节角度与目标点坐标之间的关
六自由度机器人的运动学分析
六自由度机器人的运动学分析王梦涛;张良安【摘要】For a developed 6 degree-of-freedom robot with 2,3 and 4-axis parallelled to each other, a forward kine-matics equation of the robot was acquired by establishing the link coordinate system and transformation matrix with D-H(Denavit-Hartenberg) method. Based on the idea of matrix inversion, the inverse kinematics calculation method of 6 degree-of-freedom robot was proposed, and all analytical solutions of the inverse kinematics were obtained. According to the singularity of the robot, the processing method of the inverse solution of the singular position was proposed, the forward kinematics model and inverse kinematics solution were verified by Robotics Toolbox in MATLAB software. The research results provide a basis for later trajectory planning and control of the robot, and it is a reference for the kinematics of the robot with similar structure.%针对一种自主研发的二三四轴相互平行的六自由度机器人,通过D-H(Denavit-Hartenberg)法建立机器人连杆坐标系和变换矩阵,求得正运动学方程,基于矩阵逆乘的思路进行逆运动学的求解,提出此类六自由度机器人逆运动学求解的方法,求出逆运动学的全部解析解.针对该机器人的奇异性,提出奇异位置逆解的处理方法,并用MATLAB机器人工具箱Robotics Tool-box验证了正解模型和逆解算法的正确性.研究结果为该机器人后续的轨迹规划和控制提供理论依据,对于具有相似结构的机器人运动学正逆解问题具有借鉴意义.【期刊名称】《安徽工业大学学报（自然科学版）》【年(卷),期】2016(033)004【总页数】7页(P365-371)【关键词】六自由度机器人;运动学;机器人工具箱;奇异性【作者】王梦涛;张良安【作者单位】安徽工业大学机械工程学院,安徽马鞍山243032;安徽工业大学机械工程学院,安徽马鞍山243032【正文语种】中文【中图分类】TP242.2在2015上海、苏州的工业机器人展览会上，众多机器人厂商纷纷推出其3～10kg轻载机器人产品，如日本那智不二越的MZ系列六自由度机器人、德国KUKA的LBR iiwa机器人、瑞士ABB的YuMi机器人等。
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
六轴联动机械臂运动学及动力学求解分析V0.9版随着版本的不断更新，旧版本文档中的一些笔误得到了修正，同时文档内容更丰富，仿真程序更完善。
作者朱森光Email*******************完成时间 2016-02-281引言笔者研究六轴联动机械臂源于当前的机器人产业热，平时比较关注当前热门产业的发展方向。
笔者从事的工作是软件开发，工作内容跟机器人无关，但不妨碍研究机器人运动学及动力学，因为机器人运动学及动力学用到的纯粹是数学和计算机编程知识，学过线性代数和计算机编程技术的人都能研究它。
利用业余时间翻阅了机器人运动学相关资料后撰写此文，希望能够起到抛砖引玉的作用引发更多的人发表有关机器人技术的原创性技术文章。
本文内容的正确性经过笔者编程仿真验证可以信赖。
2机器建模既然要研究机器人，那么首先要建立一个机械模型，本文将以典型的六轴联动机器臂为例进行介绍，图2-1为笔者使用3D技术建立的一个简单模型。
首先建立一个大地坐标系，一般教科书上都是以大地为XY平面，垂直于大地向上方向为Z轴，本文为了跟教科书上有所区别同时不失一般性，将以水平向右方向为X轴，垂直于大地向上方向为Y轴，背离机器人面向人眼的方向为Z轴，移到电脑屏幕上那就是屏幕水平向右方向为X轴，屏幕竖直向上方向为Y轴，垂直于屏幕向外为Z轴，之所以建立这样不合常规的坐标系是希望能够突破常规的思维定势训练在任意空间建立任意坐标系的能力。
图2-1图2-1中的机械臂，底部灰色立方体示意机械臂底座，定义为关节1，它能绕图中Y轴旋转；青色长方体示意关节2，它能绕图中的Z1轴旋转；蓝色长方体示意关节3，它能绕图中的Z2轴旋转；绿色长方体示意关节4，它能绕图中的X3轴旋转；深灰色长方体示意关节5，它能绕图中的Z4轴旋转；末端浅灰色机构示意关节6即最终要控制的机械手，机器人代替人的工作就是通过这只手完成的，它能绕图中的X5轴旋转。
这儿采用关节这个词可能有点不够精确，先这么意会着理解吧。
3运动学分析3.1齐次变换矩阵齐次变换矩阵是机器人技术里最重要的数学分析工具之一，关于齐次变换矩阵的原理很多教科书中已经描述在此不再详述，这里仅针对图2-1的机械臂写出齐次变换矩阵的生成过程。
首先定义一些变量符号，关节1绕图中Y轴旋转的角度定义为θ0，当θ0=0时，O1点在OXYZ坐标系内的坐标是(x0,y0,0)；关节2绕图中的Z1轴旋转的角度定义为θ1，图中的θ1当前位置值为+90度；定义O1O2两点距离为x1,关节3绕图中的Z2轴旋转的角度定义为θ2，图中的θ2当前位置值为-90度；O2O3两点距离为x2，关节4绕图中的X3轴旋转的角度定义为θ3, 图中的θ3当前位置值为0度；O3O4两点距离为x3，关节5绕图中的Z4轴旋转的角度定义为θ4, 图中的θ4当前位置值为-60度；O4O5两点距离为x4，关节6绕图中的X5轴旋转的角度定义为θ5, 图中的θ5当前位置值为0度。
以上定义中角度正负值定义符合右手法则,所有角度定义值均为本关节坐标系相对前一关节坐标系的相对旋转角度值（一些资料上将O4O5两点重合在一起即O4O5两点的距离x4退化为零，本文定义x4大于零使得讨论时更加不失一般性）。
符号定义好了，接下来描述齐次变换矩阵。
定义R0为关节1绕Y轴的旋转矩阵=cosθ0 s0 = sinθ0//c0R0=[c0 0 s0 00 1 0 00 c0 0-s00 0 0 1]定义T0为坐标系O1X1Y1Z1相对坐标系OXYZ的平移矩阵T0=[1 0 0 x00 1 0 y000 1 00 0 0 1]定义R1为关节2绕Z1轴的旋转矩阵R1=[c1 –s1 0 0s1 c1 0 00 0 1 00 0 0 1]定义T1为坐标系O2X2Y2Z2相对坐标系O1X1Y1Z1的平移矩阵T1=[1 0 0 x10 1 0 00 0 1 00 0 0 1]定义R2为关节3绕Z2轴的旋转矩阵R2=[c2 –s2 0 0s2 c2 0 00 0 1 00 0 0 1]定义T2为坐标系O3X3Y3Z3相对坐标系O2X2Y2Z2的平移矩阵T2=[1 0 0 x20 1 0 00 0 1 00 0 0 1]定义R3为关节4绕X3轴的旋转矩阵R3=[1 0 0 00 c3 –s3 00 s3 c3 00 0 0 1]定义T3为坐标系O4X4Y4Z4相对坐标系O3X3Y3Z3的平移矩阵T3=[1 0 0 x30 1 0 00 0 1 00 0 0 1]定义R4为关节5绕Z4轴的旋转矩阵R4=[c4 –s4 0 0s4 c4 0 00 0 1 00 0 0 1]定义T4为坐标系O5X5Y5Z5相对坐标系O4X4Y4Z4的平移矩阵T4=[1 0 0 x40 1 0 00 0 1 00 0 0 1]定义R5为关节6绕X5轴的旋转矩阵R5=[1 0 0 00 c5 –s5 00 s5 c5 00 0 0 1]以上矩阵定义中c0、c1、c2、c3、c4、c5分别为cosθ0、cosθ1、cosθ2、cosθ3、cosθ4、cosθ5的简写，s0、s1、s2、s3、s4、s5分别为sinθ0、sinθ1、 sinθ2、sinθ3、sinθ4、sinθ5的简写。
至此最终的齐次变换矩阵就可以写出来了，那就是：C=R0*T0*R1*T1*R2*T2*R3*T3*R4*T4*R53.2正运动学求解正运动学求解就是求出3.1节中齐次变换矩阵C的解析表达式，下面求解。
C=R0*T0*R1*T1*R2*T2*R3*T3*R4*T4*R5=[Nx Ox Ax PxNy Oy Ay PyNz Oz Az Pz0001]这里要注意矩阵乘法满足结合律但不满足交换律，可以先单独求出R4*T4，R3*T3，R2*T2，R1*T1，R0*T0然后再将它们相乘，即C= (R0*T0)*(R1*T1)*(R2*T2)*(R3*T3)*(R4*T4)*R5最终得出结果如下：Nx=c0c1(c2c4-c3s2s4)-c0s1(s2c4+c2c3s4)+s0s3s4=s1(c2c4-c3s2s4)+c1(s2c4+c2c3s4)NyNz=-s0c1(c2c4-c3s2s4)+s0s1(s2c4+c2c3s4)+c0s3s4Ox= c0c1(-s4c2c5-s2c3c4c5+s2s3s5) –c0s1(-s2s4c5+c2c3c4c5-c2s3s5)+s0(s3c4c5+c3s5)Oy= s1(-s4c2c5-s2c3c4c5+s2s3s5) +c1(-s2s4c5+c2c3c4c5-c2s3s5)Oz= -s0c1(-s4c2c5-s2c3c4c5+s2s3s5)+s0s1(-s2s4c5+c2c3c4c5-c2s3s5)+c0(s3c4c5+c3s5)=c0c1(c2s4s5+c3c4s2s5+c5s2s3)-c0s1(s2s4s5-c2c3c4s5-c2c5s3)+s0(-c4s3s5+c3c5)AxAy= s1(c2s4s5+c3c4s2s5+c5s2s3) +c1(s2s4s5-c2c3c4s5-c2c5s3)Az=-s0c1(c2s4s5+c3c4s2s5+c5s2s3)+s0s1(s2s4s5-c2c3c4s5-c2c5s3)+c0(-c4s3s5+c3c5)=c0c1(x4c2c4-x4c3s2s4+x3c2+x2c2)-c0s1(x4c4s2+x4c2c3s4+x3s2+x2s2)+x1c0c1+x4s0s3s4+x0c0Px=s1(x4c2c4-x4c3s2s4+x3c2+x2c2)+x1s1+c1(x4c4s2+x4c2c3s4+x3s2+x2s2)+y0PyPz=-s0c1(x4c2c4-x4c3s2s4+x3c2+x2c2)+s0s1(x4c4s2+x4c2c3s4+x3s2+x2s2)- x1s0c1+ x4c0s3s4-x0s0矩阵C就是最终的六轴联动机械臂的齐次变换矩阵，如果机械手末端相对于坐标系O5X5Y5Z5的相对坐标为U(a,b,c)，那么末端U在大地坐标系OXYZ中的坐标为：图3-1以上就是机器人正运动学的求解，Nx、Ny、Nz、Ox、Oy、Oz、Ax、Ay、Az、Px、Py、Pz表达式中的x0、y0、x1、x2、x3、x4为机械固有尺寸，θ0、θ1、θ2、θ3、θ4、θ5为六个关节的旋转角。
实际上C的子矩阵M=[Nx Ox AxNy Oy AyNz Oz Az]反应的就是末端坐标系O5X5Y5Z5的姿态，子矩阵M实际上就是轴O5X5、O5Y5、O5Z5在大地坐标系OXYZ下的方向余弦矩阵，而(Px,Py,Pz)就是点O5在大地坐标系OXYZ下的绝对坐标。
3.3逆运动学求解机器人逆运动学求解是根据末端位姿矩阵C反求六个关节的旋转角θ0、θ1、θ2、θ3、θ4、θ5的问题。
为了便于求解，这儿对C=R0*T0*R1*T1*R2*T2*R3*T3*R4*T4*R5等式进行变换，令S0=R0*T0，然后将等式两边同时左乘S0的逆S0-1得到：S0-1*C=R1*T1*R2*T2*R3*T3*R4*T4*R50 -s0-x0=[c0其中S0-10 1 0 -y0s0 0 c0 00 0 0 1]等式左边S0-1*C=[c0Nx-s0Nz c0Ox-s0Oz c0Ax-s0Az c0Px-s0Pz-x0Ny Oy Ay Py-y0S0Nx+c0Nz s0Ox+c0Oz s0Ax+c0Az s0Px+c0Pz0 0 0 1]等式右边R1*T1*R2*T2*R3*T3*R4*T4*R5=[c1(c2c4-c3s2s4)-s1(s2c4+c2c3s4) c1(-s4c2c5-s2c3c4c5+s2s3s5) c1(c2s4s5+c3c4s2s5+c5s2s3) c1(x4c2c4-x4c3s2s4+x3c2+x2c2)-s1(-s2s4c5+c2c3c4c5-c2s3s5) -s1(s2s4s5-c2c3c4s5-c2c5s3) -s1(x4c4s2+x4c2c3s4+x3s2+x2s2)+x1c1S1(c2c4-c3s2s4)+c1(s2c4+c2c3s4) s1(-s4c2c5-s2c3c4c5+s2s3s5) s1(c2s4s5+c3c4s2s5+c5s2s3) s1(x4c2c4-x4c3s2s4+x3c2+x2c2)+x1s1+c1(-s2s4c5+c2c3c4c5-c2s3s5) +c1(s2s4s5-c2c3c4s5-c2c5s3) +c1(x4c4s2+x4c2c3s4+x3s2+x2s2)S3s4 s3c4c5+c3s5 -c4s3s5+c3c5 x4s3s40 0 0 1]等式左右两个矩阵内对应元素相等于是就得到如下方程组：1c1(c2c4-c3s2s4)-s1(s2c4+c2c3s4)= c0Nx-s0Nz2s1(c2c4-c3s2s4)+c1(s2c4+c2c3s4)= Ny3s3s4= S0Nx+c0Nz4c1(-s4c2c5-s2c3c4c5+s2s3s5) -s1(-s2s4c5+c2c3c4c5-c2s3s5)= c0Ox-s0Oz5s1(-s4c2c5-s2c3c4c5+s2s3s5) +c1(-s2s4c5+c2c3c4c5-c2s3s5)= Oy6s3c4c5+c3s5= s0Ox+c0Oz7c1(c2s4s5+c3c4s2s5+c5s2s3) -s1(s2s4s5-c2c3c4s5-c2c5s3)=c0Ax-s0Az8s1(c2s4s5+c3c4s2s5+c5s2s3) +c1(s2s4s5-c2c3c4s5-c2c5s3)=Ay9-c4s3s5+c3c5= s0Ax+c0Az10c1(x4c2c4-x4c3s2s4+x3c2+x2c2) -s1(x4c4s2+x4c2c3s4+x3s2+x2s2)+x1c1= c0Px-s0Pz-x011s1(x4c2c4-x4c3s2s4+x3c2+x2c2)+x1s1+c1(x4c4s2+x4c2c3s4+x3s2+x2s2)= Py-y012x4s3s4= s0Px+c0Pz注意：以上12个方程式中c0、c1、c2、c3、c4、c5分别为cosθ0、cosθ1、cosθ2、cosθ3、cosθ4、cosθ5的简写，s0、s1、s2、s3、s4、s5分别为sinθ0、sin θ1、s inθ2、sinθ3、sinθ4、sinθ5的简写。