巳２ｓ２％＋ｃ２％
求出邑和ｃ３。
将（４．２－１）变形为：
４４４＝笱１４１４－１瓦
使式（４．４．２．２３）等号两侧推导出的矩阵中的（３，３）项相等，得
并有：
巳２ｑ％ｑ＋ｑ勤墨＋哆％
ｓｓ ２±
（４．４．２．２４）
同理根据式（４．４．２－２３）等号两侧矩阵中（２，３）（１，３）（３，２）和（３，１）
项分别相等，得：
．２９—
安徽理工大学硕士学位论文
＆＝（ａｙｑ—ｑｑ）／％ （４．４．２—２６）
ｃ４＝（ａ，ｃ２３ｑ＋口，Ｃ２３■一ａｚＳ２３）／Ｓ５
（４．４．２－２７）ｓ６＝（ｏ，ｓ２３ｑ＋ＤｙＳ２３Ｓ１＋ＯｚＣ２３）／黾
（４．４．２－２８）ｃ６＝一（ｎ，ｓ２３ｑ＋刀ｙＳ２３墨＋ｇｌｚＣ２３）／ｓｓ
（４．４．２—２９）利用各关节的旋转角的正、余弦值岛和ｑ，则各转角变量计算如下：
ｑ＝ａｔａｎ２（ｓ，，ｑ） （４．４．２－３０）
其中扛１，２…６。为了避免式（４．４．２．２６）～（４．４．２．２９）中当Ｓ５＝０（即０５＝０的退化情
形）时，计算溢出，利用式（４．４．２—３０）计算时只须考虑式（４．４．２－２６）～（４．４．２—２９）中邑的符号，即取％＝ｓ咖（％）。式（４．４．２－１７）、（４．４．２－２０）和（４．４．２－２５）各具两个计
算公式，这样可构成８组解。采用消元法【３２１求解亦可得出以上各方程。
从上面手工计算可以明显地看出，其缺点：计算量大，如果机械臂的自由度 增加，则计算量也会增加的更多。手工计算很难保证计算的准确性和计算的实时 性。
４．４．３ 程序求解ＰＵＭＡ５６０机械臂的运动方程
显然，连续运动可以认为是理想点密集的点到点（Ｐｏｉｎｔ－ｔｏ．Ｐｏｉｎｔ，ＰＰ）运动的 总和。因为，任何一条连续运动曲线，都可以离散化处理为：相隔距离很近的点 集所构成的折线来近似。这些点到点运动的总和，就构成了连续运动的曲线。这 样，只要理想点足够密集，就能用点到点运动控制的方法来实现所需精度的连续 运动，即把曲线离散化处理。
根据题目已知，机械臂的旁边有一个待加工的中空圆台形工件，上部开口。
工件高１８０ｍｍ，下底外半径１６８ｍｍ，上底外半径９６ｒａｍ，壁厚８ｍｍ。竖立地固 定在Ｘｙ平面的操作台上，底部的中心在（２１０，０，Ｏ）。根据这些条件，求出了圆台 的方程：
（ｘ一２１０）２＋ｙ２＝（１６８一导ｚ）２
、 （４．４．３．３１）在ＭＡＰＬＥ １１中绘制出圆台图形，如图４．１所示：
．３０．
图４．Ｉ特加工的工件
Ｆｉ９４ １ ＴｈｅｗｏｒｋｐｉｅｃｅｔｏｂｅⅢＤｃ∞ｓｅｄ
首先．用圆台的方程式（４ ４３－３１）和平面ｚ＝２ｚ联立求出交线。为了便于编程计 算，可将其转化为参数方程：
７３５ ５２５
４ ４
ｙ：—１＝－０５４６
ｓｉｎｆ （４４．３．３２）Ｚ
７３５ ５２５
８ ８
可以看出此交线是一椭圆曲线。
注意到，在连续运动中，可以认为是理想点密集的点到点运动的总和。根据 各点的取值，即机械臂到达的点，根据前面提出的机械臂逆运动学公式求出各步
的转换角度。
下面是住ＭＡＴＬＡＢ７０环境下，编制程序。在理想点的设置中，设定为Ｈ＝２００，
在ＭＡＰＬＥ Ｉｌ环境Ｆ，编制程序２—２ ｍｗｓ，结果发现，只将总转换角度进行ｎ次 迭代，在理想点‰到理想点啊．．处，有的理想点满足不了题日的要求：机器指令
的各个增量Ａ０，ｊ｛能取到一２。一ｌ…９ １．８。…，１．８，１．９，２这４１个离散值（即精度为０１
。，绝对值小超过２。）。
‰＝［２．３，一ｌ
２，２２，２ ３，０．６，０８】～喝１ｌ＝［２．１，０ ９，一１．只－２１，０ ３，０．８】出现这种状况的原因是：由于此交线是椭圆曲线，在每一点处曲率不同．曲 率越大越容易发生突变。为了解决此问题，采取如下办法：若迭代点产牛的机器 各转换角度即输出的备指令序列满足要求，则依次迭代；若不满足精度要求则将
安徽理Ｔ大学硕＋学位论文
一段内的转换角度进行细分，以此循环。每一步都按照题目的要求进行此判断，
直至所有的指令序列满足要求。
按照这样的解决办法，在预定点的设置中，依然设定为外层的迭代步数 胛＝２００，在需要的情况下，内层细化段数ｎ０＝１０，在ＭＡＴＬＡＢ ７．０环境下，编制 程序ｒｏｂｏｔ２ ２．ｍ。程序运行结果表明，所有的输出指令序列满足题目的要求。
由于结果有４６１个数据量，下面我们只展示了最后５个指令序列。
ｎ４５７＝【－０．９２４５，－０．０７１７，０．０７６５，１．７０６６，－０．４１７８，一１．４７３３】
ｎ４５８＝【－０．９２４３，－０．０５５５，０．０５８８，１．７７０９，－０．３６７５，一１．５４２４】
ｎ４５９＝【－０．９２４２，－０．０３９２，０．０４１１，１．８３０７，－０．３１５０，一１．６０６１】
ｎ４５９＝【－０．９２４２，－０．０３９２，０．０４１１，１．８３０７，－０．３１５０，一１．６０６１】
／＇／４６０＝【－０．９２４１，－０．０２３，０．０２３３，１．８８４５，－０．２６０６，一１．６６３１】
ｎ，ｓ１＝［－ｏ．９２４１，－０．００６７，０．００５５，１．９３ １４，－０．２０４５，一１．７ １２２】
程序运行结果表明，机械臂指尖在自动调整迭代次数的情况下，经过４６１次 的迭代，能够精确地绕交线（４．４．３．３２）一周。并且所有的输出指令序列能满足题目的 要求：即机器指令的各个增量ＡＯ，只能取到．２，．１．９，．１．８，．．．，１．８，１．９，２这４１个离散值
（即精度为０．１０，绝对值不超过２。）。