六轴运动机器人运动学求解分析_第一讲
六轴串联机器人位姿同步的实时轨迹规划
运动平滑性
分析机器人运动过程中速度、加速 度的连续变化，评估轨迹的平滑性 。
实时性
验证轨迹规划算法在不同复杂度和 动态环境下的实时性能。
结果讨论与改进方向
结果讨论
综合分析实验结果，讨论位姿同步算法和实针对算法性能、实时性和稳定性的优化措施，如引入更高效的优化算法、提高传感器精度等，以 提升六轴串联机器人的整体性能。
同步精度
评估位姿同步算法在不同 运动状态下的同步误差， 包括位置误差和姿态误差 。
实时性
分析算法在不同计算资源 下的运行时间，确保满足 实时性要求。
稳定性
测试算法在面对传感器噪 声和外部干扰时的稳定性 表现。
实时轨迹规划效果评估
轨迹跟踪精度
比较实际机器人运动轨迹与规划 轨迹的符合程度，评估轨迹规划
运动学方程
通过连乘所有连杆坐标系的变换矩阵，建立六轴串联机器 人的正运动学方程，实现由关节角度到位姿的映射。
六轴串联机器人逆运动学建模
01 02
数值解法
采用数值方法（如牛顿-拉夫逊法）求解六轴串联机器人的逆运动学问 题，通过迭代优化关节角度，使得正运动学计算得到的位姿与实际期望 位姿之间的差距最小化。
重要性
实时轨迹规划能够确保机器人在动态环境中快速适应变化，提高机器人的运动性 能和安全性能。
基于插值法的实时轨迹规划
优点
插值法计算量较小，能够实现实时性要求较高的轨迹 规划。
原理
插值法是通过在已知数据点之间插入新的数据点，以 生成平滑曲线的方法。在实时轨迹规划中，可以利用 插值法根据机器人当前位姿和目标位姿生成中间位姿 ，从而实现轨迹的平滑过渡。
硬件加速
采用专用硬件加速器或FPGA等硬件资源，对位姿同步算法中的关键计算步骤进行硬件加 速，进一步提高算法的执行速度和实时性能。
六自由度运动方程计算
n x n 0 由正运动学方程 T6 = T6 = A1 A2 A3 A4 A5 A6 = y nz 0
2
ox oy oz 0
ax ay az 0
px py 可得 pz 1
n x = C1 [C23 (C4 C5 C 6 − S 4 S 6 ) − S 23 S 5 C 6 ] + S1 (C 4 S 6 + S 4 C5 C6 ) n y = − C1 (C4 S 6 + S 4 C5 C 6 ) + S1 [C 23 (C 4 C5 C6 − S 4 S 6 ) − S 23 S 5 C6 ] n z = C23 S 5 C 6 + S 23 (C4 C5 C 6 − S 4 S 6 ) o x = C1 [−C 23 (C 4 C5 S 6 + S 4 C6 ) + S 23 S 5 S 6 ] + S1 ( C4 C6 − S 4 C5 S 6 ) o y = − C1 (C4 C6 − S 4 C5 S 6 ) + S1 [−C 23 (C 4 C5 S 6 + S 4 C6 ) + S 23 S 5 S 6 ] o z = − C23 S 5 S 6 − S 23 (C 4 C 5 S 6 + S 4 C6 ) a x = C1 (C23 C 4 S 5 + S 23 C5 ) + S1 S 4 S 5 a y = − C1 S 4 S 5 + S1 (C 23 C4 S 5 + S 23 C5 ) a z = − C23 C5 + S 23 C4 S 5 p x = C1 (l 2 C2 + d 4 S 23 ) p y = S1 (l 2 C 2 + d 4 S 23 ) p z = l 2 S 2 − d 4 C23 三 机器人逆运动学分析 S1 0 − C1 0 S3 0 − C3 0 0 1 0 0 0 0 0 1 0 1 0 0 C2 − S −1 A2 = 2 0 0 0 0 0 1 S2 C2 0 0 0 0 1 0 S4 0 C4 0 − l2 0 0 1 0 −1 0 0 C1 0 A1−1 = S1 0 C 3 0 −1 A3 = S3 0 １ 求 θ1 ox oy oz 0 ax ay az 0 px py pz 1
六轴机器人基于dh模型法的正向运动学求解
1 六轴机器人基于DH模型法的正向运动学求解六轴机器人是工业自动化中常见的一种机械手臂，具有良好的柔性和精准度。
而机器人的运动学求解是其设计和控制的重要基础。
在本文中，我们将探讨六轴机器人基于DH模型法的正向运动学求解，从简单的概念到深入的原理，让您对这一主题有一个全面的理解。
1.1 DH模型法的概念与原理DH模型法是一种描述刚体相对运动的方法，其基本原理是建立坐标系，通过旋转和平移等变换来描述刚体之间的相对运动关系。
在六轴机器人中，DH模型法通常用于描述各个关节之间的运动学关系，从而求解机器人的末端姿态。
1.2 六轴机器人的运动学参数在使用DH模型法进行运动学求解时，需要确定六轴机器人的运动学参数，包括关节长度、关节角度、关节距离等。
这些参数是求解机器人正向运动学的基础，对于精确控制和路径规划具有重要意义。
1.3 六轴机器人的正向运动学求解通过建立各个关节之间的坐标系，并结合DH参数，可以利用矩阵乘法的方式求解六轴机器人的正向运动学问题。
这一过程涉及到多个矩阵变换和坐标系转换，需要经过严谨的推导和计算。
1.4 个人观点与理解个人认为，六轴机器人基于DH模型法的正向运动学求解虽然涉及复杂的数学原理和矩阵计算，但其背后的原理是相对简单和直观的。
通过对运动学参数和坐标系的合理选择，可以有效简化运动学求解的过程，提高机器人运动的精度和灵活性。
总结回顾在本文中，我们深入探讨了六轴机器人基于DH模型法的正向运动学求解。
从概念与原理到具体计算过程，我们从简到繁地介绍了这一主题。
通过对DH模型法的概念和六轴机器人的运动学参数进行分析，我们理解了正向运动学求解的关键步骤和计算方法。
本文还共享了个人对这一主题的观点和理解，为读者提供了更深入的思考和探讨的空间。
通过本文的阅读，相信您对六轴机器人基于DH模型法的正向运动学求解有了全面、深刻和灵活的理解。
希望本文能为您在工业自动化和机器人控制方面的学习和研究提供启发和帮助。
6轴机器人正逆运动学计算公式
6轴机器人正逆运动学计算公式
正逆运动学是机器人技术中非常重要的一部分，它涉及到机器
人在空间中的位置和姿态的计算。
在机器人控制中，正运动学用于
根据关节角度计算末端执行器的位置和姿态，而逆运动学则是根据
给定的目标位置和姿态来计算关节角度。
对于6轴机器人来说，正逆运动学计算公式是非常复杂的，而
且通常需要使用矩阵运算和三维几何知识。
下面我们来简要介绍一
下这些计算公式的基本原理。
首先，对于正运动学计算，我们需要使用机器人的DH参数（Denavit-Hartenberg参数）以及每个关节的旋转矩阵来进行计算。
DH参数描述了各个关节之间的几何关系，而旋转矩阵描述了每个关
节的旋转情况。
通过这些参数，我们可以建立起整个机器人的运动
学模型，并据此计算机器人末端执行器的位置和姿态。
而对于逆运动学计算，我们则需要使用雅克比矩阵以及迭代求
解等方法来进行计算。
雅克比矩阵描述了机器人末端执行器的位置
和姿态随着关节角度的变化而变化的情况，而迭代求解则是通过不
断调整关节角度来逼近目标位置和姿态。
总的来说，6轴机器人正逆运动学计算公式是非常复杂的，需要深入的数学和物理知识以及编程技能来进行实现。
然而，掌握这些计算公式将极大地提高机器人的精度和灵活性，使其能够更好地完成各种复杂的任务。
随着机器人技术的不断发展，正逆运动学计算公式也将不断得到完善和优化，为机器人的应用提供更加强大的支持。
六轴机械臂 xyz运动算法
六轴机械臂 xyz运动算法六轴机械臂是一种具有六个自由度的机械装置，可以实现在三维空间内的运动和定位。
以下是一种常见的六轴机械臂的XYZ 运动算法：1.正向运动学算法（Forward Kinematics）：o输入：关节角度（θ1, θ2, θ3, θ4, θ5, θ6）o输出：末端执行器（笛卡尔）坐标（X, Y, Z）正向运动学算法将给定的关节角度转换为末端执行器的位姿。
这可以通过对每个关节进行坐标变换和旋转变换来实现。
具体步骤如下：o定义每个关节的DH参数（链接长度、关节间的旋转和位移等）。
o使用DH参数计算每个关节之间的齐次变换矩阵。
o将所有关节的齐次变换矩阵相乘得到末端执行器的位姿。
2.逆向运动学算法（Inverse Kinematics）：o输入：末端执行器（笛卡尔）坐标（X, Y, Z）o输出：关节角度（θ1, θ2, θ3, θ4, θ5, θ6）逆向运动学算法根据末端执行器的位姿计算相应的关节角度，以实现特定的位置和姿态。
这是一个复杂的问题，需要采用数值求解方法，如迭代或优化算法。
具体步骤如下：o定义每个关节的DH参数。
o基于末端执行器的位姿和参考坐标系，计算目标末端执行器的位姿。
o使用数值求解方法，如牛顿-拉夫逊方法或雅可比转置法，通过不断迭代调整关节角度，使得末端执行器的位姿逐渐接近目标位姿。
3.运动规划算法：XYZ运动规划算法可以通过给定的起始位置和目标位置，计算出机械臂在空间中的运动路径。
这可以通过采用合适的插值方法，如直线插值或样条插值，将机械臂的一个位置平滑过渡到另一个位置。
这样可以避免机械臂在运动中出现突变和不连续的情况。
需要注意的是，具体的XYZ运动算法可能会因不同的机械臂控制系统和机械结构而有所不同。
因此，在实际应用中，应根据具体的机械臂控制器和算法来实现适合的运动算法。
机器人学运动学分析_Kinematics
Dynamic Analysis of Mechanical Systems
Kinematics
Joints can be classified by the degrees of freedom allowed or restricted. The commonly used joints are listed in Table 1. 3.2.3 Mobility Analysis One of the basic steps in the kinematic analysis of mechanical systems is to determine the number of degrees of freedom or independent coordinates required to determine the configuration of the system. It can be shown that the number of constraints provided by a joint is equal to the number of degrees of freedom eliminated from the unconstraint system as a result of using this joint. In the planar system, the configuration of a rigid body undergoing unconstrained motion can be described by three independent coordinates due to the common constraints. Therefore, a planar system with n unconstrained movable bodies has 3 n DOF. If the system has p joints, each of which provides two constraints (note the common constraints), then the number of degrees of the system can be evaluated by the mobility (Glubler) criterion
一文读懂：工业六轴机器人D-H模型及运动学建模
第1页第一章 D-H 模型1.1 XX 机器人D-H 变换z4图1- 1 XX 机器人模型建立上述坐标系以后，坐标系i 关于坐标系1i -的位置和方向就完全由下列参数给定：a ：（相邻两个Z 轴之间的垂直的距离，即连杆的长度。
）d ：（相邻两个X 轴之间的垂直距离，即相连坐标系原点间的距离。
） α：（相邻两个Z 轴之间的夹角。
）θ：（相邻两个X 轴之间的夹角。
） 接下来进行相邻坐标系之间的坐标变换，运动顺序按照：1、将坐标系沿着轴1i Z -平移i d ，再绕着轴1i Z -旋转i θ，用齐次坐标变换描述：1'cos sin 00sin cos 00001001i i i i i i i A d θθθθ--⎡⎤⎢⎥⎢⎥=⎢⎥⎢⎥⎣⎦2、通过中间坐标系沿着轴'i X 平移i a ，并且绕着轴'i X 旋转i α，用齐次坐标变换描述为：1'1000cos sin 00sin cos 001i i i i i i i a A αααα-⎡⎤⎢⎥-⎢⎥=⎢⎥⎢⎥⎣⎦3、坐标变换通过右乘得到：1cos cos sin sin sin cos sin cos cos cos sin sin 0sin cos 01i i i i i i i i i i i i i i i i i i ia a A d θαθθαθθθαθαθαα--⎡⎤⎢⎥-⎢⎥=⎢⎥⎢⎥⎣⎦表1- 1 巨轮机器人的DH 参数表θ daα0-1 1θ1d 1a 1α 1-2 2θ 2d2a2α2-3 3θ3d 3a 3α 3-4 4θ 4d 4a 4α 4-5 5θ5d5a5α5-66θ 6d 6a 6α第3页1.2 旋转变换1、参考坐标系绕Z 轴旋转θ角的旋转矩阵为：cos sin 0sin cos 001Z R θθθθθ-⎡⎤⎢⎥=⎢⎥⎢⎥⎣⎦2、参考坐标系绕X 轴旋转α角的旋转矩阵为：1000cos sin 0sin cos x R ααααα⎡⎤⎢⎥=-⎢⎥⎢⎥⎣⎦ 得到关于两轴的旋转变换：R = [ cos(theta), -cos(alpha)*sin(theta), sin(alpha)*sin(theta)] [ sin(theta), cos(alpha)*cos(theta), -sin(alpha)*cos(theta)] [ 0, sin(alpha), cos(alpha)]1.3 正运动学变换0012345123456n T A A A A A A =1.4 逆运动学变换逆运动学是已知末端连杆的位置和方向，求机器人各个关节变量。
六自由度机器人运动学及主要构件的有限元分析
第6期2021年2月No.6February ，2021六自由度机器人运动学及主要构件的有限元分析摘要：文章以六自由度机器人为研究对象，根据实际的作业情况，对机器人进行运动学分析以及主要构件的有限元分析。
运动学分析分为正运动学分析和逆运动学分析，解决的是机器人的手臂转向何方，分析的是手部的速度、加速度和位移。
有限元分析主要是机械系统静力学分析。
对主要构件建立模型、模型简化、网格划分，根据危险工况的受力情况，分析了各构件的应力、形变等性能，确保结构设计合理。
对于工业机器人机械结构、传动等方面，运动学和有限元分析能够判断整机设计是否达到设计目标，对结构件的优化设计具有重要的意义。
关键词：六自由度；机器人；运动学；有限元分析中图分类号：TP242.2文献标志码：A 程锴（南京以禾电子科技有限公司，江苏南京210039）作者简介：程锴（1981—），男，江苏南京人，工程师，硕士；研究方向：电子产品总体结构设计。
江苏科技信息Jiangsu Science &Technology Information引言在当前科学技术不断进步和快速发展的背景下，很多先进的技术手段被广泛应用在各个领域中［1］。
特别是机器人在工业中得到广泛的应用，在实际运行过程中，类似于码垛搬运的六自由度机器人在搬运货物中节省大量劳动力，但安全性与可靠性一直备受考验。
因此，本文主要对六自由度机器人进行运动学和静力学分析［2］。
机器人运动学研究解决的是机器人的手臂转向何方，分析的是手部的速度、加速度和位移。
运动学方程是进行机器人位移分析的基本方程，也称为位姿方程。
机器人运动学分为正运动学分析和逆运动学分析。
正运动学是机器人运用各个关节角度、各个构件车长度等已知条件来判断末端执行器在三维空间中的位置；而逆运动学正好相反，它解决的是机器人需要如何运动才能使得末端执行器到达指定位置这一问题。
静力学分析用来分析结构在给定静力载荷作用下的响应。
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
六轴联动机械臂运动学求解分析第一讲作者朱森光Email zsgsoft@1引言笔者研究六轴联动机械臂源于当前的机器人产业热，平时比较关注当前热门产业的发展方向。
笔者工作主要从事软件开发跟机器人毫无关系，利用业余时间研究整理机器人技术相关的文章，希望能够起到抛砖引玉的作用引发更多的人发表有关机器人技术的原创性技术资料。
本系列文章的所有文字、图片及相关资料均为原创，内容正确性经过笔者亲自编程仿真验证可以信赖。
2机器建模2.1坐标系既然要研究机器人，那么首先要建立一个机械模型，本文将以典型的六轴联动机器臂为例进行介绍，图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轴旋转。
2.2齐次变换矩阵齐次变换矩阵是机器人技术里最重要的数学分析工具之一，关于齐次变换矩阵的原理很多教科书中已经描述在此不再详述，这里仅针对图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当前位置值为-60度；O3O4两点距离为x3，关节5绕图中的Z4轴旋转的角度定义为θ4, 图中的θ4当前位置值为-60度；O4O5两点距离为x4，关节6绕图中的X5轴旋转的角度定义为θ5, 图中的θ5当前位置值为+60度。
以上定义中角度正负值定义符合右手法则。
符号定义好了，接下来描述齐次变换矩阵。
定义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 x11 1 0 01 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*R52.3正运动学求解正运动学求解就是求出2.2节中齐次变换矩阵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)+s0s3s4s1(c2c4-c3s2s4)+c1(s2c4+c2c3s4)Ny=Nz=-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)Ax=c0c1(c2s4s5+c3c4s2s5+c5s2s3)-c0s1(s2s4s5-c2c3c4s5-c2c5s3)+s0(-c4s3s5+c3c5)Ay= s1(c2s4s5+c3c4s2s5+c5s2s3) +c1(s2s4s5-c2c3c4s5-c2c5s3)Az=-s0c1(c2s4s5+c3c4s2s5+c5s2s3)+s0s1(s2s4s5-c2c3c4s5-c2c5s3)+c0(-c4s3s5+c3c5)Px=c0c1(x4c2c4-x4c3s2s4+x3c2+x2c2)-c0s1(x4c4s2+x4c2c3s4+x3s2+x2s2)+x1c0c1+x4s0s3s4+x0c0s1(x4c2c4-x4c3s2s4+x3c2+x2c2)+x1s1+c1(x4c4s2+x4c2c3s4+x3s2+x2s2)+y0Py=Pz=-s0c1(x4c2c4-x4c3s2s4+x3c2+x2c2)+s0s1(x4c4s2+x4c2c3s4+x3s2+x2s2)+x1s0c1-x0s0矩阵C就是最终的六轴联动机械臂的齐次变换矩阵，如果机械手末端相对于坐标系O5X5Y5Z5的相对坐标为U(a,b,c)，那么末端U在大地坐标系OXYZ中的坐标为：图2-2以上就是机器人正运动学的求解，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下的绝对坐标。
2.4逆运动学求解机器人逆运动学求解是根据末端位姿矩阵C反求六个关节的旋转角θ0、θ1、θ2、θ3、θ4、θ5的问题。
为了便于求解，这儿对C=R0*T0*R1*T1*R2*T2*R3*T3*R4*T4*R5等式进行变换，令S0=R0*T0，然后将等式两边同时左乘S0的逆S0′得到：S0′*C=R1*T1*R2*T2*R3*T3*R4*T4*R5其中S0′=[c0 0 -s0 -x00 1 0 -y0s0 0 c0 00 0 0 1]c0Ox-s0Oz c0Ax-s0Az c0Px-s0Pz-x0[c0Nx-s0Nz等式左边S0′* C=Ny 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的简写。