机械臂运动学方程
7轴机械臂运动学方程
7轴机械臂运动学方程机械臂是一种能够模拟人类手臂运动的机械装置，广泛应用于工业生产线、医疗手术、空间探测等领域。
而机械臂的运动学方程则是描述机械臂运动的数学模型，通过解析运动学方程，可以准确计算机械臂的位置、速度和加速度等运动参数。
7轴机械臂是指机械臂由7个关节驱动，具有7个自由度。
每个关节都可以旋转或者转动，从而实现机械臂在空间中的各种姿态和位置变化。
为了描述机械臂的运动学特性，需要建立一套运动学方程。
机械臂的运动学方程可以分为正向运动学和逆向运动学两部分。
正向运动学是指已知机械臂各关节的角度，如何计算机械臂末端的位置和姿态。
逆向运动学则是指已知机械臂末端的位置和姿态，如何逆推出各关节的角度。
对于7轴机械臂的正向运动学方程，可以通过连续的坐标变换来实现。
首先，我们需要定义机械臂的基座坐标系和末端执行器的坐标系。
然后，通过一系列的旋转和平移变换，将基座坐标系转换到末端执行器的坐标系。
最后，通过坐标变换矩阵，可以得到机械臂末端的位置和姿态。
对于7轴机械臂的逆向运动学方程，可以通过逆解正向运动学方程来实现。
首先，已知机械臂末端的位置和姿态，我们可以通过逆变换矩阵，将末端执行器的坐标系转换到基座坐标系。
然后，通过逆解旋转和平移变换，可以得到各关节的角度。
在实际应用中，机械臂的运动学方程可以用于路径规划、碰撞检测、动力学分析等方面。
通过对机械臂的运动学进行建模和分析，可以提高机械臂的精度和效率。
然而，机械臂的运动学方程并不是一个简单的问题。
由于机械臂的关节之间存在复杂的几何约束，以及关节之间的耦合效应，导致运动学方程的求解变得困难。
因此，在实际应用中，通常会借助计算机辅助设计软件来求解机械臂的运动学方程。
总结起来，7轴机械臂的运动学方程是描述机械臂运动的重要数学模型。
通过正向运动学和逆向运动学两部分的分析，可以准确计算机械臂的位置、速度和加速度等参数。
机械臂的运动学方程不仅在工业自动化领域有着广泛的应用，还对于机器人技术的发展起着重要的推动作用。
机械臂正运动学方程的DH表示法及逆运动学
关节变量
• 在图（a）中， 角表示绕Z轴的旋转角，d表示在
Z轴上两条相邻的公垂线之间的距离，a表示每一 条公垂线的长度（也叫关节偏移量），角 表示
两个相邻的Z轴之间的角度（也叫关节扭转）
8
坐标变换
• 假设现在位于本地坐标系
xn zn ，那么通
过四步标准运动即可到达下一个本地坐标 系 xn 1 zn 1 旋转平移平移旋转
1
前提
• 假设机器人由一系列关节和连杆组成。这 些关节可能是滑动（线性）的或者是旋转 （转动）的，它们可以按任意的顺序放置 并处于任意的平面。 • 连杆可以是任意的长度（包括零），它可 能被弯曲或扭曲，也可能位于任意平面上。 • 所以任何一组关节和连杆都可以构成一个 我们想要建模和表示的机器人。
2
R 1 2
18
n1
其中n是关节数
19
20
l a d 3 21 2 4 ii
21
22
• 在机器人的基座和手之间的总变换为：
R
TH A1 A2 A3 A4 A5 A6
S12 C12 l4 S12 l3 S12 l6 S12 l5C4C12 l3 S12 l4C12 l6C12 l5C4 S12 0 l2 l5 S 4 0 1
C n1 S n1 S C n1 n 1 0 0 0 0 0 0 1 0 0 1 0 0 0 0 1 0 0 1 0 0 0 0 1 0 0 0 1 d n1 0 0 0 1
=
25
26
• 如果关节是旋转的，Z轴位于按右手规则旋转的
方向。绕Z轴的旋转角 是关节变量；
• 如果关节是滑动的，Z轴为沿直线运动的方向。
运动学逆解公式
运动学逆解公式
运动学逆解是指已知机器人末端执行器的位置、姿态和运动学参数，求解机器人各关节的角度。
运动学逆解公式的具体形式取决于机器人的类型和结构，以下是几种常见机器人的运动学逆解公式：
1. 二自由度平面机械臂的运动学逆解公式：
θ1 = atan2(y, x) - acos((l1^2 + l2^2 - r^2)/(2*l1*l2))
θ2 = -acos((x^2 + y^2 - l1^2 - l2^2)/(2*l1*l2))
其中，θ1和θ2分别为机械臂两个关节的角度，x和y为末端执行器的位置坐标，l1和l2为机械臂两个关节的长度，r为末端执行器到机械臂起点的距离。
2. 三自由度空间机械臂的运动学逆解公式：
θ1 = atan2(y, x)
θ3 = acos((x^2 + y^2 + z^2 - l1^2 - l2^2 - l3^2)/(2*l2*l3))
k1 = l2 + l3*cos(θ3)
k2 = l3*sin(θ3)
θ2 = atan2(z, sqrt(x^2 + y^2)) - atan2(k2, k1)
其中，θ1、θ2和θ3分别为机械臂三个关节的角度，x、y和z为末端执行器的位置坐标，l1、l2和l3为机械臂三个关节的长度。
3. 六自由度工业机器人的运动学逆解公式：
由于六自由度工业机器人的运动学逆解公式比较复杂，这里不再给出具体公式。
通常采用数值计算方法求解，如牛顿-拉夫逊法、雅可比逆法等。
需要注意的是，运动学逆解公式只能求解机器人的正解，即机器人末端执行器的位置、姿态和运动学参数必须是合法的。
如果末端执行器的位置、姿态和运动学参数不合法，就无法求解出机器人各关节的角度。
机械臂运动学.
机械臂运动学基础1、机械臂的运动学模型机械臂运动学研究的是机械臂运动，而不考虑产生运动的力。
运动学研究机械臂的位置，速度和加速度。
机械臂的运动学的研究涉及到的几何和基于时间的内容，特别是各个关节彼此之间的关系以及随时间变化规律。
典型的机械臂由一些串行连接的关节和连杆组成。
每个关节具有一个自由度，平移或旋转。
对于具有n个关节的机械臂，关节的编号从1到n，有n +1个连杆，编号从0到n。
连杆0是机械臂的基础，一般是固定的，连杆n上带有末端执行器。
关节i连接连杆i和连杆i-1。
一个连杆可以被视为一个刚体，确定与它相邻的两个关节的坐标轴之间的相对位置。
一个连杆可以用两个参数描述，连杆长度和连杆扭转，这两个量定义了与它相关的两个坐标轴在空间的相对位置。
而第一连杆和最后一个连杆的参数没有意义，一般选择为0。
一个关节用两个参数描述，一是连杆的偏移，是指从一个连杆到下一个连杆沿的关节轴线的距离。
二是关节角度，指一个关节相对于下一个关节轴的旋转角度。
为了便于描述的每一个关节的位置，我们在每一个关节设置一个坐标系，对于一个关节链，Denavit和Hartenberg提出了一种用矩阵表示各个关节之间关系的系统方法。
对于转动关节i，规定它的转动平行于坐标轴z i-1，坐标轴x i-1对准从z i-1到z i的法线方向，如果z i-1与z i相交，则x i-1取z i−1×z i的方向。
连杆，关节参数概括如下：●连杆长度a i沿着x i轴从z i-1和z i轴之间的距离;●连杆扭转αi从z i-1轴到zi轴相对x i-1轴夹角;●连杆偏移d i从坐标系i-1的原点沿着z i-1轴到x i轴的距离;●关节角度θi x i-1轴和x i轴之间关于z i-1轴的夹角。
对于一个转动关节θi 是关节变量，d i 是常数。
而移动关节d i 是可变的，θi 是恒定的。
为了统一，表示为ii iq d θ⎧=⎨⎩转动关节移动关节 运用Denavit-Hartenberg （DH ）方法，可以将相邻的两个坐标系之间的变换关系表示为一个4x4的齐次变换矩阵1cos sin cos sin sin cos sin cos cos cos sin sin 0sin cos 01ii i i i i i i i ii ii i i i iii a a A d θθαθαθθθαθαθαα--⎡⎤⎢⎥-⎢⎥=⎢⎥⎢⎥⎣⎦上式表示出了坐标系i 相对于坐标系i-1的关系。
六关节机械臂拉格朗日动力学方程
六关节机械臂拉格朗日动力学方程六关节机械臂是一种多自由度机械结构，常见于工业制造、医疗器械等领域，具有灵活、高精度的特点。
在进行机械臂运动控制时，拉格朗日动力学方程是一种重要的数学工具，可以描述机械臂的运动学和动力学特性。
本篇文章将详细介绍六关节机械臂拉格朗日动力学方程的推导过程和应用。
**一、机械臂的构造**六关节机械臂由6个关节连接而成，每个关节可以进行转动运动。
机械臂的末端往往安装有工具或夹具，用于完成各种任务。
机械臂上的每个关节都有一个旋转轴和一个驱动器，通过控制驱动器的运动来控制机械臂的姿态和位置。
**二、运动学分析**在进行动力学分析之前，首先需要对机械臂的运动进行数学建模，得到机械臂各关节的运动学方程。
常用的方法是使用旋转矩阵和欧拉角来描述机械臂的姿态。
将机械臂的姿态表示为旋转矩阵，可以得到机械臂末端位姿与各个关节角度之间的关系。
**三、拉格朗日动力学方程的推导**拉格朗日动力学方程是用于描述机械系统的运动学和动力学特性的重要数学工具。
其基本思想是从系统的运动学模型出发，推导出系统的动力学模型。
1.定义广义坐标和广义速度：根据机械臂的运动学模型，引入广义坐标和广义速度来描述系统的状态，广义坐标用于表示机械臂各关节的角度，广义速度用于表示机械臂各关节的角速度。
2.动能和势能的计算：根据机械臂的构造和运动特点，可以计算出机械臂的动能和势能。
机械臂的动能可以分解为各个关节的动能之和，势能可以表示为机械臂的重力势能。
3.拉格朗日函数的建立：定义拉格朗日函数为系统的动能减势能，即L = T - V。
4.拉格朗日方程的推导：根据拉格朗日函数的定义，可以通过对拉格朗日函数求导来得到系统的运动方程，即拉格朗日方程。
拉格朗日方程描述了系统的动力学特性，包括系统的运动学关系和动力学关系。
**四、应用**通过求解六关节机械臂的拉格朗日动力学方程，可以得到机械臂的运动方程。
这些方程可以用于机械臂的运动规划、轨迹跟踪、运动控制等领域。
六轴机械臂 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运动算法可能会因不同的机械臂控制系统和机械结构而有所不同。
因此，在实际应用中，应根据具体的机械臂控制器和算法来实现适合的运动算法。
机械臂动力学方程
机械臂动力学方程
机械臂动力学方程
1．什么是机械臂动力学方程?
机械臂动力学方程是一组描述机械臂在空间运动时的运动学模型，是一组力学方程的汇总，是由机械学理论，动力学学理论和多关节系统学理论所确定的表达式。
它提供了有关机械臂运动状态下结构参数和运动参数的量化表达。
2．机械臂动力学方程的基本构成
机械臂动力学方程的基本构成包括：（1）动力学方程组，即机械结构形式下力学方程，用于描述机械臂的运动状态；（2）动力学约束方程组，其内容包括施加的力、质量、加速度等参数的表达式；（3）构型角度的约束关系，是指机械臂构型角度之间关系的表达式；（4）外力和转矩作用于机械臂的力角关系；（5）机械臂参数，如连杆长度、半轴长度、杆比等；（6）机械臂空间位姿，如机械臂的位置、四元数、速度和加速度等。
3．机械臂动力学方程的优点
（1）便于计算机控制。
机械臂动力学方程提供了用于分析机械臂运动状态的一种抽象的表达式，有助于编写精确的控制模型；
（2）提高机械臂控制精度。
使用机械臂动力学方程，可以更好地描述机械臂空间运动状态，从而使机械臂控制精度更高；
（3）降低设备损坏率。
利用机械臂动力学方程可以更全面地评估机械臂的极限位置，从而防止机械臂发生破坏；
（4）有助于分析系统传动特性。
使用机械臂动力学方程，可以更好地研究机械臂系统传动特性，更好地进行系统能量计算。
4．机械臂动力学方程的应用
机械臂动力学方程在工业应用广泛，主要包括：（1）机械手、抓手等自动装配系统；（2）机床仿形系统；（3）车架装配系统；（4）机器手臂把柄装配系统；（5）汽车冷却系统的液力传动装配；（6）空间机器人；（7）机器视觉检测等。
图1给出一个二连杆机械臂,试求其运动方程式
图1给出一个二连杆机械臂,试求其运动方程式
1.机器人系统模型
假设一个二连杆机械臂结构如图所示。
其中，
机械臂的质量主要富集于机器人关节处，分别为：、；机械臂连杆长度分别为：、；机械臂关节运动为纯旋转，所以仅存在为角度的广义坐标：、；重力加速度为：。
根据定义
拉格朗日方程：
其中，
L为拉格朗日函数，而K为系统广义动能，P为系统广义位能。
为连杆子系统索引号（）；为第个子系统的动能和位能的坐标（位移和角度），为相应的广义速度（线速度和角速度）；为第个子系统的广义力（力和力矩）。
2.计算各个连杆的动能及位能
该二连杆机器人系统可将机器人拆解为两个单独连杆（连杆1和连杆2）的子系统。
2.1连杆1的动能
2.2连杆1的位能
2.3连杆2的动能
2.4连杆2的位能
2.5系统总能量
总动能：
总位能
3.拉格朗日平衡法求动力学方程
二连杆机械臂的拉格朗日函数L可以计算得：
让L对分别对进行求导：
；
；
机械臂各关节力矩分别为，根据拉格朗日方程可以得到：
整理成动力学方程，
写成矩阵形式
通常上述矩阵形式可以用二阶非线性微分方程描述：
其中，分别为机器人广义坐标、广义速度、广义加速度；
为机械臂惯性矩阵；表示离心力和科氏力矩阵；表示机器人重力矩阵；表示机器人广义力。
常用性质：
性质1：是一个斜对称矩阵；
性质2：是一个对称正定矩阵。
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
机械手臂的运动学公式推导1. 仿人机器人手臂模型● 仿人机器人的手臂有6个自由度，肩部（shoulder ）3个，肘部（elbow ）2个，腕部（wrist ）1个，如图1所示。
● 机器人手臂的几何尺寸（mm ）：上臂长度：216 小臂长度：173.5● 关节的运动范围（右手）：如表1所示。
表1 关节运动范围⑴ 参考坐标系为了对仿人机器人进行控制，同时也便于描述机器人的动作状态，必须建立适当的初始坐标系。
我们设定机器人手臂的初始姿态：大臂从肩垂直向下，小臂向前平伸，与大臂成90。
参考坐标系（实验室坐标系）的设定以机器人本身的初始位置与实验室坐标系相一致的原则设定，如图2所示。
X 轴：以机器人初始（状态）位置的右侧方向作为实验室坐标系的X 轴； y 轴：设定y 轴使其为右手系坐标系，即正前方为y 轴正向。
Z 轴：以机器人初始（状态）位置的上方向作为实验室坐标系的Z 轴；按D-H 坐标建立的方法，各个关节的轴线与各关节坐标系的Z 轴共线.(2) 关节坐标系各关节坐标系的建立如图3所示。
12 3 4 5 6图1 手臂模型 X 5 XOZ Y图2 参考坐标系X 3 O 3 Z 3Y 3Z 4 O 4Y 4X 4O 5Z 5Y 5Z 6 O 6Y 6 X 6 X 1 O 1 Z 1Y 1 Y 2 O 2 X 2Z 2shoulder 1、2、3elbow wrist 4、5 6 图3 关节坐标系（3）连杆参数连杆参数列表如表2所示。
表2 连杆参数连杆之间的齐次变换矩阵为：⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡---=----------100001111111111i i i i i i i i c d c s c s s s d s c c c s a s c T i i i i i i i ii i i αααααααα从而可以确定：⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡-=100001000000111101c s s c T ⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡--=100000010000222212c s s c T ⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡---=10000100003303323c s l s c T ⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡--=100000010000444434c s s c T⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡---=100000100005515545c s l s c T ⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡--=10000010000666656c s s c TT T T T T T T 56453423120106= =[ (((cos(t1)*cos(t2)*cos(t3)+sin(t1)*sin(t3))*cos(t4)+cos(t1)*sin(t2)*sin(t4))*cos(t5)-(-cos(t1)*c os(t2)*sin(t3)+sin(t1)*cos(t3))*sin(t5))*cos(t6)-(-(cos(t1)*cos(t2)*cos(t3)+sin(t1)*sin(t3))*sin(t4)+cos(t1)*sin(t2)*cos(t4))*sin(t6),-(((cos(t1)*cos(t2)*cos(t3)+sin(t1)*sin(t3))*cos(t4)+cos(t1)*sin(t2)*sin(t4))*cos(t5)-(-cos(t1)*cos (t2)*sin(t3)+sin(t1)*cos(t3))*sin(t5))*sin(t6)-(-(cos(t1)*cos(t2)*cos(t3)+sin(t1)*sin(t3))*sin(t4)+cos(t1)*sin(t2)*cos(t4))*cos(t6),-((cos(t1)*cos(t2)*cos(t3)+sin(t1)*sin(t3))*cos(t4)+cos(t1)*sin(t2)*sin(t4))*sin(t5)-(-cos(t1)*cos(t2)*sin(t3)+sin(t1)*cos(t3))*cos(t5), (-(cos(t1)*cos(t2)*cos(t3)+sin(t1)*sin(t3))*sin(t4)+cos(t1)*sin(t2)*cos(t4))*l1-cos(t1)*sin(t2)*l0] [ (((sin(t1)*cos(t2)*cos(t3)-cos(t1)*sin(t3))*cos(t4)+sin(t1)*sin(t2)*sin(t4))*cos(t5)-(-sin(t1)*co s(t2)*sin(t3)-cos(t1)*cos(t3))*sin(t5))*cos(t6)-(-(sin(t1)*cos(t2)*cos(t3)-cos(t1)*sin(t3))*sin(t4)+s in(t1)*sin(t2)*cos(t4))*sin(t6),-(((sin(t1)*cos(t2)*cos(t3)-cos(t1)*sin(t3))*cos(t4)+sin(t1)*sin(t2)*sin(t4))*cos(t5)-(-sin(t1)*cos(t 2)*sin(t3)-cos(t1)*cos(t3))*sin(t5))*sin(t6)-(-(sin(t1)*cos(t2)*cos(t3)-cos(t1)*sin(t3))*sin(t4)+sin(t1)*sin(t2)*cos(t4))*cos(t6), -((sin(t1)*cos(t2)*cos(t3)-cos(t1)*sin(t3))*cos(t4)+sin(t1)*sin(t2)*sin(t4))*sin(t5)-(-sin(t1)*cos(t2)*sin(t3)-cos(t1)*cos(t3))*cos(t5), (-(sin(t1)*cos(t2)*cos(t3)-cos(t1)*sin(t3))*sin(t4)+sin(t1)*sin(t2)*cos(t4))*l1-sin(t1)*sin(t2)*l0] [((-sin(t2)*cos(t3)*cos(t4)+cos(t2)*sin(t4))*cos(t5)-sin(t2)*sin(t3)*sin(t5))*cos(t6)-(sin(t2)*cos(t3)*sin(t4)+cos(t2)*cos(t4))*sin(t6), -((-sin(t2)*cos(t3)*cos(t4)+cos(t2)*sin(t4))*cos(t5)-sin(t2)*sin(t3)*sin(t5))*sin(t6)-(sin(t2)*cos(t3)*sin(t4)+cos(t2)*cos(t4))*cos(t6), -(-sin(t2)*cos(t3)*cos(t4)+cos(t2)*sin(t4))*sin(t5)-sin(t2)*sin(t3)*cos(t5), (sin(t2)*cos(t3)*sin(t4)+cos(t2)*cos(t4))*l1-cos(t2)*l0] [0,0,0,1]⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡-==-1000010000cos sin 00sin cos 111110110θθθθTT ⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡---==-100000100cos 0sin 0sin 0cos 222211221θθθθTT ⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡----==-10000100cos 0sin 0sin 0cos 0333312332l TT θθθθ⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡---==-1000100cos 0sin 0sin 0cos 444413443θθθθTT⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡----==-10000100cos 0sin 0sin 0cos 1555514554l TT θθθθ ⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡---==-1000100cos 0sin 0sin 0cos 666615665θθθθT T 以“6”为参考，1、2、3三个关节交点“0”的位置由4、5、6三个关节决定，因此有26363266P T P =其中，⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡=1266z y x p p p P 已知；263P 为23T 的第4列，即⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡-=1000263l P T T T T 43546563= p=[]Tl s s l c l c c s c s l s l c s s c c P T 1)()(054160464561604645626363-+--+-=1222266266+++=z y x T p p p P P120142120+-+=l l c l l p P T a l l p p p l l c zy x =---+=1222212042令2tan 4θ=u ，则a u u c =+-=22411，有aau +-±=11 a a +-=11arctan24θ或aa+-+-=11arctan 24πθ（4） θ4的范围为 –150----30 由054l s s p z -=得： 405s l p s z-=其中04≠s令u =2tan5θ，则2512sin u u+=θ，即40212s l p uu z-=+ )11arctan(211122420405242040402-±-=-±-==++s l p p s l s l p p s l u u p sl u zz zz zθ)11arctan(2)11arctan(224204052420405-++-=-+-=s l p p s l s l p p s l zz zz πθθ （5）θ5的范围为 –180----180 160460456l s l c s l c c c p x +-= 则有：0)22()(0450042045=-+-++l c c p u l l c u l c c p x x)(2))((4)22()22(0450450452004004l c c p l c c p l c c p l l c l l c u x x x +-+--±--=因此6θ有两个值： 04522425424200046)12()(arctan2l c c p p c c c c l l l c x x+-++-+--=θ或4522425424200046)12()(arctan 2l c c p p c c c c l l l c x x+-++-+-+-=πθ二、手腕方位的反解321θθθ、、决定手腕的方位。
T T T p a o n p a o n p a o n z z z z y y y y x x x x 10213210=⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡ 或⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡------+---+=10000022121323132131321323132131321102132l c s s s c s s c c s c s cs s c c c s s c c c s s s c c c T T T3tan θ-=xy a a 当0sin 2≠θ时),(2tan 3x y a a a -=θ1tan θ-=zzn o ),(2tan 1z z n o a -=θ12sin tan θθz za o =)sin ,(2tan 12θθz z a o a =。