《六自由度机械臂姿态监测系统及方法.pdf》由会员分享，可在线阅读，更多相关《六自由度机械臂姿态监测系统及方法.pdf（18页珍藏版）》请在专利查询网上搜索。
1、10申请公布号CN104182614A43申请公布日20141203CN104182614A21申请号201410360729X22申请日20140725G06F19/00201101G08C17/02200601H04L29/06200601G01C21/1820060171申请人山东建筑大学地址250101山东省济南市历城区临港开发区凤鸣路72发明人刘存根鲁守银王涛田娅孙丽萍74专利代理机构济南圣达知识产权代理有限公司37221代理人张勇54发明名称六自由度机械臂姿态监测系统及方法57摘要本发明公开了六自由度机械臂姿态监测系统及方法，包括机械臂整体系统的若干个监测节点，每个监测节点处安装有。
2、监测装置，监测装置有监控终端通过ZIGBEE无线通信模块控制启停，ZIGBEE无线通信模块与中央处理单元通信，中央处理单元与监测装置通信，用于接收监测装置所监测的监测节点的信息。利用无线传感网络对机械臂整体系统进行姿态监测，通过监测终端和若干监测节点实现同时对多个节点的监测，克服了传统只对单个点姿态监测的缺陷。51INTCL权利要求书3页说明书11页附图3页19中华人民共和国国家知识产权局12发明专利申请权利要求书3页说明书11页附图3页10申请公布号CN104182614ACN104182614A1/3页21六自由度机械臂姿态监测系统，其特征是，包括布置在机械臂整体系统的若干个监测节点，每个。
3、监测节点处安装有监测装置，每个监测装置有监控终端通过ZIGBEE无线通信模块控制启停，ZIGBEE无线通信模块与中央处理单元通信，中央处理单元与监测装置通信，中央处理单元接收监测装置所监测的监测节点的信息。2如权利要求1所述的六自由度机械臂姿态监测系统，其特征是，所述监测装置包括用于测量机械臂关节的加速度值的三轴加速度传感器，用于测量机械臂关节的地磁感应强度分量的三轴地磁传感器及用于测量械臂关节的角速度值的三轴陀螺仪。3如权利要求1所述的六自由度机械臂姿态监测系统，其特征是，所述若干个监测节点包括机械臂基座的第一监测节点、机械臂关节处的第二监测节点、机械臂关节处的第三监测节点、机械臂关节处的第。
4、四监测节点、机械臂关节处的第五监测节点、机械臂关节处的第六监测节点及机械臂关节处的第七监测节点。4如权利要求1所述的六自由度机械臂姿态监测系统的监测方法，其特征是，包括以下步骤步骤一中央处理单元读取三轴地磁传感器测量出的机械臂各关节的三维地磁感应强度分量值MX、MY、MZ、三轴加速度传感器测量出的机械臂各关节的三维加速度分量值X、Y、Z及三轴陀螺仪测量出的机械臂各关节的角速度；步骤二对读取的各关节的三维地磁感应强度分量、各关节的三维加速度分量值利用四元数算法进行归一化处理，对读取的陀螺仪角速度进行偏差补偿后的四元数向量进行归一化处理；步骤三将步骤二归一化处理后所得四元数转换成欧拉角即旋转角；将。
5、各部分进行多次测量测得的多个旋转角取其均值；步骤四根据步骤三中的旋转角、机械臂的臂长及各关节坐标系通过三角函数计算得出机械臂最终姿态。5如权利要求4所述的监测方法，其特征是，所述步骤一中还包括测量得到三维加速度值分量X、Y、Z及三维地磁感应分量MX、MY、MZ，对分量进行校准，校准的目的是为了去除由外界环境和测量过程中造成的误差，即实验前先分别对多次测得的加速度值和地磁感应三个分量取平均值，以此平均值作为标定值校准当次测量值的准确性；计算加速度参考值ACC和地磁感应强度参考值MAG；其中6如权利要求4所述的监测方法，其特征是，所述步骤二中具体包括当加速度测量值和参考值ACC的偏差小于设定阈值时。
6、，加速度的四元数表示通过归一化处理得到否则，由对地坐标系中的加速度通过四元数转换得到其中各式中参数含义如下加速度测量值其中SATSAB,TSGT，SAT是经过误差补偿的线性加速度，SAB,T是重力补偿后物体的线性加速度，BA,T是由低频偏移引起的偏差，EA,T为零均值白权利要求书CN104182614A2/3页3噪声，SGT的取值取决于传感器定位；SAT为加速度测量值的归一化表示即加速度的四元数表示，为值的幅值，X,T、Y,T、Z,T分别为时间T时的三个方向的加速度分量，为在测量时间内的单位四元数表示，为的共轭，EZA0001。7如权利要求4所述的监测方法，其特征是，所述步骤二中还包括当地磁感。
7、应强度测量值和参考值MAG的偏差小于设定阈值时，地磁感应强度的四元数表示通过归一化处理得到否则，由对地坐标系中的地磁通过四元数转换得到其中，地磁感应强度测量值其中，SMT为地磁场的磁感应强度，DM,T为磁场干扰，BM,T为磁场偏差，EM,T为噪声，MX,T、MY,T、MZ,T分别为时间T时的三个方向的地磁感应强度分量，SMT为时间T时的地磁感应强度测量值的归一化表示即地磁感应强度的四元数表示，SMTT为测量时间内的地磁感应强度，为在时间T时的单位四元数表示，为的共轭；根据高斯牛顿法则，寻找使取最小值，以便于求取使陀螺仪角速度误差最小的其中为误差函数；求解陀螺仪偏差漂移补偿四元数梯度向量，并对四。
8、元数梯度向量进行归一化处理其中为传感器坐标系相对于地坐标系的陀螺仪方向的四元数的变化率，为单位四元数的变化。8如权利要求4所述的监测方法，其特征是，所述对读取的陀螺仪角速度进行偏差补偿，角度误差角度偏差补偿后的传感器坐标系下的角度向量其中，SE,TT为陀螺仪轴中补偿后的角误差，表示互补滤波融合后被估计方向的四元数表示的共轭，被用作陀螺仪偏差漂移补偿的四元数表示变化率，SB,TT为角偏移，为抵消非零均值陀螺仪测量误差的收敛率，STT为角速度；权利要求书CN104182614A3/3页4计算由陀螺仪数据估算的四元数梯度向量其中为陀螺仪测量的四元数变化率，为陀螺仪的被估计方向的单位四元数表示；计算经。
9、过互补滤波器对陀螺仪偏差漂移进行的补偿，最终得到的角速度四元数向量其中并进行归一化处理；其中互补滤波器融合的被估计方向的四元数表示，是互补滤波融合后被估计方向的四元数变化率，为陀螺仪测量的四元数变化率，为滤波器增益。9如权利要求4所述的监测方法，其特征是，所述步骤三中将步骤二所得四元数转换成欧拉角，由于机械臂每个部分只能进行单一运动方式如只能是俯仰、旋转、摆动中的一种，所以求取的旋转角只能是由相应的运动产生的横摆角、俯仰角、滚摆角中的一种角度；四元数与欧拉角的转换关系如下若四元数QWXYZT，则旋转角度可由下式得出所述步骤三中，将各部分进行多次测量测得的多个旋转角取其均值，以确保测量的准确性；。
10、在每个关节处最终的旋转角其中为已求取的旋转角，N为测量的次数。10如权利要求4所述的监测方法，其特征是，所述步骤四的具体步骤为通过旋转角和臂长在相应坐标系中的投影测得各部分的姿态后，通过各部分的叠加得到机械臂最终的姿态。权利要求书CN104182614A1/11页5六自由度机械臂姿态监测系统及方法技术领域0001本发明涉及六自由度机械臂姿态监测系统及方法。背景技术0002机械臂在工程机械技术领域具有重要作用，机械臂姿态不精确的问题会导致机械臂的控制难以满足实际需求。对机械臂整体系统的姿态监测可以使机械臂操作精准，以使机械臂达到预定位置、实施预定动作。由此机械臂整体系统的姿态监测的正常工作对具有。
11、机械臂的工程机械的正常工作同样具有重要作用。0003传统的机械臂姿态监测是根据机械臂的结构特点和运动学约束，对机械臂的运动学进行分析。采用DENAVITHARTENBERGDH法对机械臂操作空间进行描述，在考虑机械臂运动学约束的基础上，得到以关节角度为变量的正运动学模型。通过分析正运动学模型的可解性，采用矩阵逆乘的解析法求解机械臂的正运动学模型，进而得到了机械臂逆运动学的解。利用DH方法对机械臂进行分析建模首先，给每个关节指定一个参考坐标系；然后，确定如何实现任意2个相邻坐标系之间的变换；最后，写出机械臂的总变换矩阵。0004综上所述，传统方法的不足可以总结为传统的机械臂整体系统的姿态监测方法。
12、是通过将机械臂各个关节进行DH建模，然后通过各个轴之间复杂的矩阵变换得出总的变换矩阵从而推导出末端的姿态。这种方式存在计算量大，计算过程容易出错，降低姿态监测的精度，且工作复杂效率低等问题。发明内容0005为解决现有技术存在的不足，本发明公开了六自由度机械臂姿态监测系统及方法，本发明从机械臂整体结构为出发点，在机械臂各个关节之间协调工作时，求取某个臂的姿态坐标时不需要经过复杂的坐标变换得出，可以直接经过姿态监测模块测量计算得出。本发明将复杂的矩阵变换转化为三角函数的计算，直接通过角度相关计算推导出末端的姿态。简化了计算过程，提高了测量精度。同时姿态监测模块可直接安装在多自由度机械臂的关节处，不。
13、需要改变机械臂原有的安装来进行复杂的安装过程，并且其结构简单、抗干扰性强。0006为实现上述目的，本发明的具体方案如下0007六自由度机械臂姿态监测系统，包括布置在机械臂整体系统的若干个监测节点，每个监测节点处安装有监测装置，每个监测装置有监控终端通过ZIGBEE无线通信模块控制启停，ZIGBEE无线通信模块与中央处理单元通信，中央处理单元与监测装置通信，中央处理单元接收监测装置所监测的监测节点的信息。0008所述监测装置包括用于测量机械臂关节的加速度值的三轴加速度传感器，用于测量机械臂关节的地磁感应强度分量的三轴地磁传感器及用于测量械臂关节的角速度值的三轴陀螺仪。0009所述若干个监测节点包。
14、括机械臂基座的第一监测节点、机械臂关节处的第二监测说明书CN104182614A2/11页6节点、机械臂关节处的第三监测节点、机械臂关节处的第四监测节点、机械臂关节处的第五监测节点、机械臂关节处的第六监测节点及机械臂关节处的第七监测节点。0010六自由度机械臂姿态监测方法，包括以下步骤0011步骤一中央处理单元读取三轴地磁传感器测量出的机械臂各关节的三维地磁感应强度分量值MX、MY、MZ、三轴加速度传感器测量出的机械臂各关节的三维加速度分量值X、Y、Z及三轴陀螺仪测量出的机械臂各关节的角速度；0012步骤二对读取的各关节的三维地磁感应强度分量、各关节的三维加速度分量值利用四元数算法进行归一化处。
15、理，对读取的陀螺仪角速度进行偏差补偿后的四元数向量进行归一化处理；0013步骤三将步骤二归一化处理后所得四元数转换成欧拉角即旋转角；将各部分进行多次测量测得的多个旋转角取其均值；0014步骤四根据步骤三中的旋转角、机械臂的臂长及各关节坐标系通过三角函数计算得出机械臂最终姿态。0015所述步骤一中还包括测量得到三维加速度值分量X、Y、Z及三维地磁感应分量MX、MY、MZ，对分量进行校准，校准的目的是为了去除由外界环境和测量过程中造成的误差，即实验前先分别对多次测得的加速度值和地磁感应三个分量取平均值，以此平均值作为标定值校准当次测量值的准确性。计算加速度参考值ACC和地磁感应强度参考值MAG；0。
16、016其中0017所述步骤二中具体包括0018当加速度测量值和参考值ACC的偏差小于设定阈值时，加速度的四元数表示通过归一化处理得到否则，由对地坐标系中的加速度通过四元数转换得到0019其中各式中参数含义如下0020加速度测量值其中SATSAB,TSGT，SAT是经过误差补偿的线性加速度，SAB,T是重力补偿后物体的线性加速度，BA,T是由低频偏移引起的偏差，EA,T为零均值白噪声，SGT的取值取决于传感器定位；SAT为加速度测量值的归一化表示即加速度的四元数表示，为加速度测量值的幅值，X,T、Y,T、Z,T分别为时间T时的三个方向的加速度分量，为在测量时间内的单位四元数表示，为的共轭，EZA。
17、0001。0021当地磁感应强度测量值和参考值MAG的偏差小于设定阈值时，地磁感应强度的四元数表示通过归一化处理得到否则，由对地坐标系中的地磁通过四元数转换得到说明书CN104182614A3/11页70022其中，地磁感应强度测量值其中，SMT为地磁场的磁感应强度，DM,T为磁场干扰，BM,T为磁场偏差，EM,T为噪声，MX,T、MY,T、MZ,T分别为时间T时的三个方向的地磁感应强度分量，SMT为时间T时的地磁感应强度测量值的归一化表示即地磁感应强度的四元数表示，SMTT为测量时间内的地磁感应强度，为在时间T时的单位四元数表示，为的共轭。0023根据高斯牛顿法则，寻找使取最小值，以便于求取。
18、使陀螺仪角速度误差最小的其中为误差函数；0024求解陀螺仪偏差漂移补偿四元数梯度向量，并对四元数梯度向量进行归一化处理0025其中为传感器坐标系相对于地坐标系的陀螺仪方向的四元数的变化率，为单位四元数的变化。0026所述对读取的陀螺仪角速度进行偏差补偿，角度误差角度偏差补偿后的传感器坐标系下的角度向量0027其中，SE,TT为陀螺仪轴中补偿后的角误差，表示互补滤波融合后被估计方向的四元数表示的共轭，被用作陀螺仪偏差漂移补偿的四元数表示变化率，SB,TT为角偏移，为抵消非零均值陀螺仪测量误差的收敛率，STT为角速度。0028计算由陀螺仪数据估算的四元数梯度向量其中为陀螺仪测量的四元数变化率，为陀。
19、螺仪的被估计方向的单位四元数表示；0029计算经过互补滤波器对陀螺仪偏差漂移进行的补偿，最终得到的角速度四元数向量其中并进行归一化处理；0030其中互补滤波器融合的被估计方向的四元数表示，是互补滤波融合后被估计方向的四元数变化率，为陀螺仪测量的四元数变化率，为滤波器增益；0031所述步骤三中将步骤二所得四元数转换成欧拉角。由于机械臂每个部分只能进行说明书CN104182614A4/11页8单一运动方式如只能是俯仰、旋转、摆动中的一种，所以求取的旋转角只能是由相应的运动产生的横摆角、俯仰角、滚摆角中的一种角度。其中，、分别为绕Z轴、Y轴、X轴的旋转角度对应横摆角、俯仰角、滚摆角。0032四元数与。
20、欧拉角的转换关系如下0033若四元数QWXYZT，则旋转角度可由下式得出00340035所述步骤四中，将各部分进行多次测量测得的多个旋转角取其均值，以确保测量的准确性。0036在每个关节处最终的旋转角其中为已求取的旋转角，N为测量的次数。0037所述步骤四的具体步骤为通过旋转角和臂长在相应坐标系中的投影测得各部分的姿态后，通过各部分的叠加得到机械臂最终的姿态；0038在机械臂基座的监测节点100处建坐标系XYZ，作为机械臂整体系统的基准，其中A0、A1、A2、A3、A4、A5分别为相邻两个关节之间的臂长；0039在机械臂关节处的第一监测节点100处建坐标系X0Y0Z0，作为基准，第一监测节点对。
21、应的位姿；00400041在机械臂关节处的第二监测节点101处建坐标系X1Y1Z1，第一关节旋转0，第二监测节点对应的位姿；00420043在机械臂关节处的第三监测节点102处建坐标系X2Y2Z2，第二关节旋转1，第三监测节点对应的位姿；说明书CN104182614A5/11页900440045在机械臂关节处的第四监测节点103处建坐标系X3Y3Z3，第三关节旋转2，第四监测节点对应的位姿；00460047在机械臂关节处的第五监测节点104处建坐标系X4Y4Z4，第四关节旋转3，第五监测节点对应的位姿；00480049在机械臂关节处的第六监测节点105处建坐标系X5Y5Z5，第五关节旋转4，第。
22、六监测节点对应的位姿；00500051在机械臂关节处的第七监测节点106处建坐标系X6Y6Z6，第六关节旋转5，第七监测节点对应的位姿；00520053将各个关节的姿态累加得到机械臂的最终姿态为0054说明书CN104182614A6/11页100055本发明的有益效果00561使用多个数字式传感器和高速中央处理单元建立姿态监测模块优化算法，将复杂的矩阵变换转化为三角函数的计算，简化了计算过程，提高了测量精度。00572姿态监测模块可直接安装在多自由度机械臂的关节处，不需要改变机械臂原有的安装来进行复杂的安装过程，并且其结构简单、抗干扰性强。00583利用无线传感网络对机械臂整体系统进行姿态监。
23、测，通过监测终端和若干监测节点实现同时对多个节点的监测，克服了传统只对单个点姿态监测的缺陷。附图说明0059图1为根据本发明实施例所述的安装监测装置的机械臂；0060图2为根据本发明实施例所述的监测装置系统结构图；0061图3为根据本发明实施例所述的监测方法流程示意图；0062图4为根据本发明实施例所述的机械臂的坐标系图；0063图5为根据本发明实施例所述的地坐标和传感器坐标及其基于单位四元数的转换图；0064图中，201中央处理单元、202加速度传感器、203地磁传感器、204陀螺仪、205ZIGBEE通信模块、206监测终端、100机械臂基座的第一监测节点、101机械臂关节处的第二监测节点。
24、、102机械臂关节处的第三监测节点、103机械臂关节处的第四监测节点、104机械臂关节处的第五监测节点、105机械臂关节处的第六监测节点、106机械臂关节处的第七监测节点。具体实施方式0065下面结合附图对本发明进行详细说明0066如图1所示，本发明具体实施例包括中央处理单元201、加速度传感器202、地磁传感器203、陀螺仪204、ZIGBEE通信模块205和监测终端206，如附图2所示。上述各模块均集成在一小型电路板上，各个电路板固定在被测机械臂上，如附图1所示。在由小型电路板测得机械臂各关节姿态角后结合臂长通过建立坐标系，如附图4所示，经过计算得出姿态坐标。本申请的机械臂是美国CRAFT。
25、公司的产品，型号是GRIPS。0067中央处理单元201选择TI公司的低功耗单片机MSP430F149，其连接简单外围电路构成最小系统，用于采集和处理加速度传感器202、地磁传感器203和陀螺仪204的数据，最终通过相关算法，转换成机械臂的姿态数据。加速度传感器202采用ADI公司的三轴加速度计ADXL345，具有体积小、重量轻、功耗低等优点，通过SPI接口和主控制器通信。地磁传感器203选择HONEYWELL公司的三轴地磁传感器HMC5843，具有如下优势可以实现X、Y、Z说明书CN104182614A107/11页11三轴磁场同时测试，测量范围更广，体积更小，集成度更高。通过I2C接口和中。
26、央处理单元相连接。陀螺仪204选用三轴数字式传感器ITG3205，其通过I2C接口和中央处理单元104相连接。ZIGBEE通信模块204选择CC2530。具有低功耗，组网方便等特点。0068系统工作过程如下系统监测终端发送的控制指令后，读取三轴加速度传感器测量出的机械臂各关节的三维加速度值X、Y、Z，读取三轴地磁传感器测量出的机械臂各关节的三维地磁感应强度分量MX、MY、MZ，结合四元数算法进行归一化处理，同时将进行陀螺仪角速度偏差补偿后的四元数向量进行归一化处理，最后将所得四元数转换成欧拉角，各个部分的旋转角经过多次测量后取其均值，以确保测量的准确性。再结合臂长根据各关节坐标系通过三角函数相。
27、关计算得出机械臂最终姿态。最终通过ZIGBEE无线通信模块将各个节点的数据传输至监测终端进行数据的的实时显示与存储，中央处理单元接收监测装置所监测的监测节点的信息并进行数据的处理。在已知旋转角和臂长的情况下计算得出机械臂的位姿。0069如附图3所示，姿态监测模块计算旋转角度包括以下步骤0070根据装置初始状态，测量得到三维加速度值分量X、Y、Z及三维地磁感应分量MX、MY、MZ，对分量进行校准，校准的目的是为了去除由外界环境和测量过程中造成的误差，即实验前先分别对多次测得的加速度值和地磁感应三个分量取平均值，以此平均值作为标定值校准当次测量值的准确性，消除误差对测量值的影响。然后计算加速度参考。
28、值ACC和地磁感应强度参考值MAG；其中0071当加速度测量值和参考值ACC的偏差小于设定阈值时，加速度的四元数表示通过归一化处理得到否则，由对地坐标系中的加速度通过四元数转换得到0072其中各式中参数含义如下0073如图5所示，加速度测量值其中SATSAB,TSGT，SAT是经过误差补偿的线性加速度，SAB,T是重力补偿后物体的线性加速度，BA,T是由低频偏移引起的偏差，EA,T为零均值白噪声，SGT的取值取决于传感器定位；SAT为加速度测量值的归一化表示即加速度的四元数表示，为值的幅值，X,T、Y,T、Z,T分别为时间T时的三个方向的加速度分量，为在测量时间内的单位四元数表示，为的共轭，E。
29、ZA0001。0074当地磁感应强度测量值和参考值MAG的偏差小于设定阈值时，地磁感应强度的四元数表示通过归一化处理得到否则，由对地坐标系中的地磁通过四元数转换得到0075其中，地磁感应强度测量值其中，SMT为地磁场的磁感应说明书CN104182614A118/11页12强度，DM,T为磁场干扰，BM,T为磁场偏差，EM,T为噪声，MX,T、MY,T、MZ,T分别为时间T时的三个方向的地磁感应强度分量，SMT为时间T时的地磁感应强度测量值的归一化表示即地磁感应强度的四元数表示，SMTT为测量时间内的地磁感应强度，为在时间T时的单位四元数表示，为的共轭。0076根据高斯牛顿法则，寻找使取最小值，。
30、以便于求取使陀螺仪角速度误差最小的其中为误差函数；0077求解陀螺仪偏差漂移补偿四元数梯度向量，并对四元数梯度向量进行归一化处理0078其中为传感器坐标系相对于地坐标系的陀螺仪方向的四元数的变化率，为单位四元数的变化。0079所述对读取的陀螺仪角速度进行偏差补偿，角度误差角度偏差补偿后的传感器坐标系下的角度向量0080其中，SE,TT为陀螺仪轴中补偿后的角误差，表示互补滤波融合后被估计方向的四元数表示的共轭，被用作陀螺仪偏差漂移补偿的四元数表示变化率，SB,TT为角偏移，为抵消非零均值陀螺仪测量误差的收敛率，STT为角速度。0081计算由陀螺仪数据估算的四元数梯度向量其中为陀螺仪测量的四元数变。
31、化率，为陀螺仪的被估计方向的单位四元数表示；0082计算经过互补滤波器对陀螺仪偏差漂移进行的补偿，最终得到的角速度四元数向量其中并进行归一化处理；0083其中互补滤波器融合的被估计方向的四元数表示，是互补滤波融合后被估计方向的四元数变化率，为陀螺仪测量的四元数变化率，为滤波器增益；0084最后，将所得四元数转换成欧拉角。由于机械臂每个部分只能进行单一运动方式如只能是俯仰、旋转、摆动中的一种，所以求取的选转角只能是由相应的运动产生的横摆说明书CN104182614A129/11页13角、俯仰角、滚摆角中的一种角度。其中，、分别为绕Z轴、Y轴、X轴的旋转角度。若QWXYZT，则旋转角度可由下式得出。
32、00850086将各部分进行多次测量测得的多个旋转角取其均值，以确保测量的准确性。0087在每个关节处最终的旋转角其中为已求取的旋转角，N为测量的次数。0088在参考相关算法得到欧拉角的情况下结合各部分的臂长，各部分建立相应的坐标系，利用三角函数相关计算得出机械臂各关节的姿态。具体算法如下0089选择相应坐标系通过旋转角和臂长在相应坐标系中的投影测得各部分的姿态后，通过各部分的叠加得到机械臂最终的姿态。0090在机械臂基座的监测节点100处建坐标系X0Y0Z0，作为机械臂整体系统的基准，其中A0、A1、A2、A3、A4、A5分别为相邻两个关节之间的臂长；0091在机械臂关节处的第一监测节点10。
33、0处建坐标系X0Y0Z0，作为基准，第一监测节点对应的位姿；00920093在机械臂关节处的第二监测节点101处建坐标系X1Y1Z1，第一关节旋转0，第二监测节点对应的位姿；00940095在机械臂关节处的第三监测节点102处建坐标系X2Y2Z2，第二关节旋转1，第三监测节点对应的位姿；说明书CN104182614A1310/11页1400960097在机械臂关节处的第四监测节点103处建坐标系X3Y3Z3，第三关节旋转2，第四监测节点对应的位姿；00980099在机械臂关节处的第五监测节点104处建坐标系X4Y4Z4，第四关节旋转3，第五监测节点对应的位姿；01000101在机械臂关节处的第六监测节点105处建坐标系X5Y5Z5，第五关节旋转4，第六监测节点对应的位姿；01020103在机械臂关节处的第七监测节点106处建坐标系X6Y6Z6，第六关节旋转5，第七监测节点对应的位姿；01040105将各个关节的姿态累加得到机械臂的最终姿态为0106说明书CN104182614A1411/11页15说明书CN104182614A151/3页16图1说明书附图CN104182614A162/3页17图2图3说明书附图CN104182614A173/3页18图4图5说明书附图CN104182614A18。