CN102581849A - 一种基于nc代码的工业机器人轨迹规划方法 - Google Patents

Abstract

一种基于NC代码的工业机器人轨迹规划方法，涉及一种机器人轨迹规划方法，尤其涉及一种以通用NC代码为工作语言的机器人轨迹规划方法。为克服传统机器人工作语言的复杂性和不通用性，本发明以通用NC代码为工作语言，来实现工业机器人的雕刻、喷漆等基本功能。采用“虚拟刀具”的概念，将每条NC代码信息快速转化成机器人逆解所需参数。利用三次五次复合插值法，对机器人的连续路径进行规划，获得精确的路径。

Description

一种基于NC代码的工业机器人轨迹规划方法

技术领域

本发明涉及一种机器人轨迹规划方法，尤其涉及一种以通用NC代码为工作语言的机器人轨迹规划方法。

背景技术

众所周知，数控技术经过几十年的发展，不管是在发达国家还是在发展中国家的制造业中都占有举足轻重的地位。NC代码作为数控机床的工作语言已经在全球范围内各大数控系统制造商中形成共识，NC代码的来源也变得异常广泛，除了现有主流CAM软件诸如MasterCAM、UG、Pro/e等外，国内外很多学者都开发专用系统在特殊环境下获得NC代码，《机床与液压》在2010年第16期的《基于VC++的NC代码自动生成程序开发》一文就阐述了如何从DXF文件中提取信息并将其转化成NC代码。

目前国内外主流工业机器人品牌有瑞士的ABB，德国的KUKA，日本的川崎、FANUC、安川，我国以蒋新松院士为首的沈阳自动化研究所研发的新松机器人等，各个品牌的机器人都有各自的工作语言，这些工作语言是不能通用的，并且掌握起来比较困难。如果能将NC代码作为机器人工作语言，那么将是对工业机器人产业化的巨大推动。国内外对工业机器人以NC代码作为工作语言也略有有研究。《机器人学》第26卷第6期《基于G代码的工业机器人的自动编程》一文中，上海交通大学刘为志先生主要着重介绍了圆弧的插补，并且顺利应用在典型的工业机器人Motoman UP6上。在TheInternational Journal of Advanced Manufacturing Technology 2005年第26卷中载有希腊人S.Mitsi的一篇文章Off-ling Programming of an Industrial Robot For Manufacturing，该篇文章介绍了作者开发的一套机器人离线编程系统，该系统就有轨迹规划、仿真和NC代码生成模块，并可将该系统生成的NC代码作为机器人工作语言输入到机器人控制系统来控制机器人完成任务。美中不足的是，该系统产生的NC代码专用性较强，并不是通用NC代码，所谓通用NC代码，是指制造业通常使用的NC代码，经过后处理可以直接由CAM生产的代码，由于专用性限制，该系统产生的NC代码不能在现行三轴或五轴机床上运行。基于以上的阐述，开发一套摒弃现有工业机器人编程模式，并以通用NC代码作为其工作语言的机器人系统也就非常有必要，而本发明专利涉及的正是基于NC代码工业机器人的轨迹规划。

发明内容

本发明要解决的问题是：现有机器人工作语言种类多，使用多较为复杂，为了充分利用NC语言的通用性和易获得性，本发明的目的是以现行广泛应用的NC代码为机器人工作语言的来实现工业机器人的抓取、焊接、喷漆、轻量切削等基本功能。

本发明的技术方案为：一种基于NC代码的工业机器人轨迹规划方法，工业机器人的末端工具为固接在机器人第六自由度的刚体，以通用NC代码为工业机器人的工作语言，将工业机器人的末端工具作为虚拟刀具，在编程坐标系下，设置五个点O、P、Q、S、T来描述虚拟刀具的基本特征，点O设置在编程坐标系的原点，点P设置在编程坐标系的Z轴上，其他三个点Q、S、T根据末端工具的具体特征来设置，其中，设置Q点要使得刀具安装后Q点与机器人手腕中心重合，且直线QS与直线ST互相垂直；将每条NC代码的数据通过虚拟刀具的五点由编程坐标系变换到机器人坐标系，进行轨迹规划，并根据所述变换到机器人坐标系后的五个点得到机器人手腕中心点的位置和手腕姿态，从而获得机器人的逆解参数，进行机器人逆运动学求解。

当虚拟刀具的五点变换到机器人坐标系后，利用三次五次插值法进行轨迹规划，所述三次五次插值法为：先取路径上的四个点O、P、Q、S进行三次插值获得中间两点P、Q的速度、加速度信息，再取P、Q、S、T四个点进行三次插值获得前两点P、Q的速度、加速度信息，将两次获取的P、Q点的速度、加速度取平均值，作为P、Q点最终的速度和加速度，加上P、Q两点的位移信息，共6个条件，插值一条五次曲线，以此保证控制点的位移以及控制点导数连续，即在平滑性的基础上获得精确的数据点。

工业机器人为五轴机床时，包括以下步骤：

S1)获得NC代码：先设置后处理器后生产代码，设转动轴为B、C轴，后处理器中设置第四轴、第五轴偏置均为0，其基本格式为：

N**G**X**Y**Z**B**C**F**S**

其中，N为行号，G为插补方式，X、Y、Z为编程坐标系中，工件表面上一点的坐标，B、C为B、C轴转角，F为进给速度，S为主轴转速；

S2)将虚拟刀具的5个点分别绕Y轴和Z轴旋转相应的角度θ，式(1)是绕Y轴旋转的旋转矩阵，式(2)是某点(x，y，z)绕Y轴旋转变换得到(x′，y′，z′)：

$\mathrm{Rot}(y,\mathrm{\θ})=\left[\begin{array}{ccc}\mathrm{c\θ}& 0& \mathrm{s\θ}\\ 0& 1& 0\\ -\mathrm{s\θ}& 0& \mathrm{c\θ}\end{array}\right]---\left(1\right)$

$\left[\begin{array}{c}{x}^{\′}\\ y^{\′}\\ z^{\′}\end{array}\right]=\mathrm{Rot}(y,\mathrm{\θ})\left[\begin{array}{c}x\\ y\\ z\end{array}\right]---\left(2\right)$

其中，sθ＝sinθ，cθ＝cosθ；

S3)(x′，y′，z′)加上刀尖点，即工件表面点的位置坐标(X，Y，Z)，变换后得到在编程坐标系{Piece}下虚拟刀具的5个点坐标(x_P，y_P，z_P)，如式(3)：

$\left[\begin{array}{c}{x}_P\\ y_P\\ z_P\end{array}\right]=\left[\begin{array}{c}{x}^{\′}\\ y^{\′}\\ z^{\′}\end{array}\right]+\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]---\left(3\right);$

S4)经过机器人对工件进行标定，得到编程坐标系{Piece}和机器人坐标系{Base}之间的变换关系，求出{Base}坐标系下虚拟刀具的5个点坐标(x_B，y_B，z_B)，如式(4)所示：

$\left[\begin{array}{c}{x}_B\\ y_B\\ z_B\end{array}\right]=R_P^B\left[\begin{array}{c}{x}_P\\ y_P\\ z_P\end{array}\right]+P_{P_0}^B---\left(4\right)$

其中，是{Piece}坐标系相对于{Base}坐标系的旋转矩阵，

为{Piece}坐标系原点在{Base}坐标系中的描述；

S5)在{Base}坐标系下，利用三次五次插值法进行轨迹规划：

CAM后处理器处理出来的NC代码具有连续的数据点，取作业过程中连续4个数据通过三次插值法进行轨迹插值，以相邻点的弦长l为横坐标 $l=\sqrt{{(x_2-x_1)}^2+{(y_2-y_1)}^2+{(z_2-z_1)}^2},$ 分别以X(l)、Y(l)、Z(l)、B(l)、C(l)为纵坐标进行计算，

对于X(l)，三次曲线分别设为：

x₁(l)＝a₁*l³+b₁*l²+c₁*l+d₁

(5)

x₂(l)＝a₂*l³+b₂*l²+c₂*l+d₂

将4个插值点数据代入：

X(l₁)＝a₁*l₁ ³+b₁*l₁ ²+c₁*l₁+d₁

X(l₂)＝a₁*l₂ ³+b₁*l₂ ²+c₁*l₂+d₁

(6)

X(l₃)＝a₁*l₃ ³+b₁*l₃ ²+c₁*l₃+d₁

X(l₄)＝a₁*l₄ ³+b₁*l₄ ²+c₁*l₄+d₁

4个方程4个未知数，则参数a₁、b₁、c₁、d₁可求，软件编程时写成矩阵形式，求出参数a₁、b₁、c₁、d₁，代入原方程，则可求出P、Q两点的一阶导数、二阶导数：

X′_P1(l₂)＝3a₁*l₂ ²+2b₁*l₂+c₁

X′_Q1(l₃)＝3a₁*l₃ ²+2b₁*l₃+c₁(7)

X″_P1(l₂)＝6a₁*l₂+2b₁

X″_Q1(l₃)＝6a₁*l₃+2b₁

同理可得，从x₂(l)获得的P、Q两点的速度加速度为：

X′_P2(l₂)＝3a₂*l₂ ²+2b₂*l₂+c₂

X′_Q2(l₃)＝3a₂*l₃ ²+2b₂*l₃+c₂(8)

X″_P2(l₂)＝6a₂*l₂+2b₂

X″_Q2(l₃)＝6a₂*l₃+2b₂

取两次P、Q的速度加速度的平均值作为P、Q两点最终的速度和加速度，得：

X′(l₂)＝(X′_P1(l₂)+X′_P2(l₂))/2

X′(l₃)＝(X′_Q1(l₃)+X′_Q2(l₃))/2(9)

X″(l₂)＝(X″_P1(l₂)+X″_P2(l₂))/2

X″(l₃)＝(X″Q₁(l₃)+X″_Q2(l₃))/2

至此，有了P、Q点共6个条件，设五次方程：

将P、Q点6个条件代入：

X′(l₂)＝f′_X(l₂)

X″(l₂)＝f″_X(l₂)

X′(l₃)＝f′_X(l₃)(11)

X″(l₃)＝f″_X(l₃)

6个方程6个未知数，同理可求出α、β、γ、μ、

和ζ每个参数都是l₂l₃的表达式，软件编程时写成矩阵形式求解；

至此，求出六个系数后可用等步长法进行插值进算：

Y(l)、Z(l)、B(l)、C(l)为纵坐标的计算方法与X(l)计算方法相同，全部计算结束后，NC代码上的信息转化到虚拟刀具上五个点在{Base}坐标系下的坐标；

S6)根据{Base}坐标系下虚拟刀具的5个点坐标，求出机器人手腕中心Q点位姿：Q点就是手腕中心点的位置坐标；Q点和S点确定了接近矢量

S点和T点决定了方向矢量

法线矢量

从而三个矢量确定手腕中心点的姿态；

S7)根据S6)中的逆解参数，即S6)中的Q点坐标和三个矢量进行机器人逆运动学求解。

本发明首次提出并使用了“虚拟刀具”这个概念，将工业机器人的末端工具做为虚拟刀具，使得NC代码点方便快捷的转化为机器人逆解所需要的参数。机器人末端工具形式多样，比如喷漆用的喷枪、旋杯；焊接切割用的焊枪；切削用的电主轴；抓取用的气动抓手等。对于上述一般末端工具，都是固接在机器人第六自由度的刚体，这里只需五个点就能够描述其基本特征。根据五个点来得到机器人末端，即第四五六自由度的交点的位置和姿态数据，从而获得了逆解数据。

本发明具有的有益效果是：

1)NC代码在制造业中非常流行，并且即使是最底层的操作工都能编写NC代码，本发明基于通用NC代码设计，使得数控领域广泛使用的NC代码作为工业机器人的工作语言成为现实，克服了机器人工作语言较难掌握的复杂性，这样就不用再去学习那些诸如ABB等机器人的繁杂程序编写，能够快速推动工业机器人的进一步发展；

2)将末端工具看作虚拟道具，通过相互位置固定的五点来表征虚拟道具，在轨迹规划中不受实际末端工具的形状影响，只需考虑所设定的点即可；

3)本发明使用三次五次插值法，获得精确的插值数据，并且由仿真结果显示机器人运动平稳，无突变。

附图说明

图1是本发明NC代码的信息向机器人手腕转化的数据流程。

图2是本发明虚拟刀具在编程坐标系中的描述，这里将编程坐标系与工件坐标系重合。

图3是本发明虚拟刀具在编程坐标系中的一个实例，即虚拟的主轴。

图4是本发明机器人坐标系下轨迹规划流程图。

图5是本发明三次五次插值法原理图。

图6是本发明工业机器人仿真切削过程中的两个瞬间。

图7为本发明工业机器人、真实刀具以及虚拟刀具的设置示意图。

图8为本发明虚拟刀具的安装示意图。

具体实施方式

本发明根据工业机器人的末端工具为固接在机器人第六自由度的刚体这一点，以通用NC代码为工业机器人的工作语言，将工业机器人的末端工具做为虚拟刀具，在编程坐标系下用五个点来描述虚拟刀具的基本特征，设置五个点O、P、Q、S、T，点O设置在编程坐标系的原点，点P设置在编程坐标系的Z轴上，其他三个点Q、S、T根据末端工具的具体特征来设置，设置Q点要使得刀具安装后Q点与手腕中心C点重合，且直线QS与直线ST互相垂直，如图3；将每条NC代码的数据变换，使编程坐标系下虚拟刀具的五点变换到机器人坐标系，将五点描述的虚拟刀具在机器人坐标系下进行轨迹规划，然后根据所述五个点来得到机器人末端工具的位置和姿态数据，从而获得机器人的逆解参数，进行机器人逆运动学求解。

如图7，显示了工业机器人、真实刀具以及虚拟刀具，机器人手腕中心不是在机器人第六轴的断面，而是在第四五六轴的交点，如图示C点。逆解参数是C点的坐标和C点处的三个矢量，真实的刀具是安装在第六轴端面的，本发明设置的Q点不是设置在刀具的端面，而是当刀具安装上之后Q点恰好能与C点重合，如图8所示，这样所求出来的Q点的坐标和矢量也就是手腕中心C点的，也就是逆解参数了。因此，设置Q点时，要加上机器人第六轴的长度，使得Q点与机器人手腕中心C点重合，本发明所设定的虚拟刀具其实就是通过相互位置保持不变的五个点来表征的。

下面通过具体实施例来说明本发明的实施步骤。

S1)如图1，在p1阶段获得NC代码，此处以双摆头五轴NC代码来说明，在后处理构造器中设置第四轴，即B轴在XZ平面转动，设置第五轴，即C轴在XY平面转动，且第四轴、第五轴偏置均为0，其基本格式为：

N**G**X**Y**Z**B**C**F**S**

其中，N为行号，G为插补方式，X、Y、Z为编程坐标系中，工件表面上一点的坐标，B、C为B、C轴转角，F为进给速度，S为主轴转速；(X，Y，Z)为刀尖点坐标，转角B、C确定了刀具的矢量；

S2)将虚拟刀具的5个点分别绕Y轴和Z轴旋转相应的角度，式(1)是绕Y轴旋转的旋转矩阵，式(2)是某点(x，y，z)绕Y轴旋转变换得到(x′，y′，z′)。

$\mathrm{Rot}(y,\mathrm{\θ})=\left[\begin{array}{ccc}\mathrm{c\θ}& 0& \mathrm{s\θ}\\ 0& 1& 0\\ -\mathrm{s\θ}& 0& \mathrm{c\θ}\end{array}\right]---\left(1\right)$

$\left[\begin{array}{c}{x}^{\′}\\ y^{\′}\\ z^{\′}\end{array}\right]=\mathrm{Rot}(y,\mathrm{\θ})\left[\begin{array}{c}x\\ y\\ z\end{array}\right]---\left(2\right)$

S3)如图1中的p2阶段，设置末端工具在编程坐标系中的初始坐标，然后经过步骤S1)、S2)，获得虚拟刀具五个点在编程坐标系下的坐标。

(x′，y′，z′)加上刀尖点的位置坐标，变换后可以得带在编程坐标系{Piece}下虚拟刀具的5个点坐标(x_P，y_P，z_P)，如式(3)。

$\left[\begin{array}{c}{x}_P\\ y_P\\ z_P\end{array}\right]=\left[\begin{array}{c}{x}^{\′}\\ y^{\′}\\ z^{\′}\end{array}\right]+\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]---\left(3\right)$

S4)如图1中的p3阶段，经过步骤S3)获得虚拟刀具五个点在机器人坐标系下的坐标。经过机器人对工件进行标定，就可以知道编程坐标系{Piece}和机器人坐标系{Base}之间的变换。实际操作中，工作台坐标系是机器人坐标系和工件坐标系之间的一个中间坐标系，若工作台是固定的，根据标定算法，此处可以忽略工作台坐标系，直接标定出编程坐标系与机器人坐标系之间的关系。求出{Base}坐标系下虚拟刀具的5个点坐标(x_B，y_B，z_B)，如式(4)所示：

$\left[\begin{array}{c}{x}_B\\ y_B\\ z_B\end{array}\right]=R_P^B\left[\begin{array}{c}{x}_P\\ y_P\\ z_P\end{array}\right]+P_{P_0}^B---\left(4\right)$

其中，

是{Piece}坐标系相对于{Base}坐标系的旋转矩阵，

为{Piece}坐标系原点在{Base}坐标系中的描述。

S5)在{Base}坐标系下，利用三次五次插值法进行轨迹规划。

S6)根据{Base}坐标系下虚拟刀具的5个点坐标就可以求出机器人手腕中心Q点位姿：Q点就是手腕中心点的位置坐标；Q点和S点确定了接近矢量

S点和T点决定了方向矢量

而

从而三个矢量确定可手腕中心点的姿态。

如图1中的p4阶段，经过步骤S4)，S5)，S6)获得机器人末端姿态。在步骤S5)中，利用三次五次插值法，对NC代码的连续轨迹进行插值，流程见图4，如图7所示，就是先取路径上的四个点O、P、Q、S进行三次插值获得中间两点P、Q的速度、加速度信息，再取P、Q、S、T四个点进行三次插值获得前两点P、Q的速度、加速度信息，将两次P、Q点的速度、加速度取平均值作为P、Q点的速度和加速度，加上P、Q两点的位移信息，共6个条件，插值一条五次曲线，，如图5中的放大图中，中间线所示，这样，既保证了早控制点的位移又保证了控制点导数连续，即在平滑性的基础上获得更精确的数据点，如图5。

CAM后处理器处理出来的NC代码具有连续的数据点，取作业过程中连续4个数据进行轨迹插值，以相邻点的弦长l为横坐标

分别以X(l)、Y(l)、Z(l)、B(l)、C(l)为纵坐标进行计算。现以X(l)为例。

三次曲线可分别设为：

可设上述三次曲线分别为：

x₁(l)＝a₁*l³+b₁*l²+c₁*l+d₁

(5)

x₂(l)＝a₂*l³+b₂*l²+c₂*l+d₂

将4个插值点数据代入：

X(l₁)＝a₁*l₁ ³+b₁*l₁ ²+c₁*l₁+d₁

X(l₂)＝a₁*l₂ ³+b₁*l₂ ²+c₁*l₂+d₁

(6)

X(l₃)＝a₁*l₃ ³+b₁*l₃ ²+c₁*l₃+d₁

X(l₄)＝a₁*l₄ ³+b₁*l₄ ²+c₁*l₄+d₁

4个方程4个未知数，则参数a₁、b₁、c₁、d₁可求。软件编程时写成矩阵形式，此处不给出a、b、c、d关于l₁l₂l₃l₄的表达式。求出参数a₁、b₁、c₁、d₁，代入原方程，则可求出P、Q两点的一阶导数、二阶导数：

X′_P1(l₂)＝3a₁*l₂ ²+2b₁*l₂+c₁

X′_Q1(l₃)＝3a₁*l₃ ²+2b₁*l₃+c₁(7)

X″_P1(l₂)＝6a₁*l₂+2b₁

X″_Q1(l₃)＝6a₁*l₃+2b₁

同理可得，从x₂(l)获得的P、Q两点的速度加速度为：

X′_P2(l₂)＝3a₂*l₂ ²+2b₂*l₂+c₂

X′_Q2(l₃)＝3a₂*l₃ ²+2b₂*l₃+c₂(8)

X″_P2(l₂)＝6a₂*l₂+2b₂

X″_Q2(l₃)＝6a₂*l₃+2b₂

取两次P、Q的速度加速度的平均值作为P、Q两点最终的速度和加速度，得：

X′(l₂)＝(X′_P1(l₂)+X′_P2(l₂))/2

X′(l₃)＝(X′_Q1(l₃)+X′_Q2(l₃))/2(9)

X″(l₂)＝(X″_P1(l₂)+X″_P2(l₂))/2

X″(l₃)＝(X″_Q1(l₃)+X″_Q2(l₃))/2

至此，有了P、Q点共6个条件，设五次方程：

将P、Q点6个条件代入：

X′(l₂)＝f′_X(l₂)

X″(l₂)＝f″_X(l₂)

X′(l₃)＝f′_X(l₃)(11)

X″(l₃)＝f″_X(l₃)

6个方程6个未知数，同理可求出α、β、γ、μ、

和ζ每个参数都是l₂l₃的表达式，这里仍然不给出具体表达式，软件编程时写成矩阵形式求解。

至此，求出六个系数后可用等步长法进行插值进算：

Y(l)、Z(l)、B(l)、C(l)为纵坐标的计算方法与X(l)计算方法相同，全部计算结束后，NC代码上的信息转化到虚拟刀具上五个点在{Base}坐标系下的坐标，这五个点之间的相对位置是不变的，虚拟刀具实际就是通过相互位置保持不变的这五个点来描述。

S7)根据设置，Q点与机器人手腕中心C点重合，所求出来的Q点的坐标和矢量也就是手腕中心C点的，也就是逆解参数了，如图1的p5阶段，根据步骤S6)中的逆解参数，即S6)中的Q点坐标和三个矢量进行机器人逆运动学求解，进而获得了六关节转角。

本发明中，通过轨迹规划获得路径上更为密集的点的坐标，由每一个点逆解获得机器人六个关节的转角，实现机器人的控制。

图6是仿真系统导入NC代码后，模拟切削过程中两个瞬间。根据本发明的方法，轨迹规划后仿真的控制结果显示过程平稳，这里用两幅瞬间截图以示之，验证了引入虚拟刀具的正确性，同时也验证规划算法的真确性。

Claims (3)

1.一种基于NC代码的工业机器人轨迹规划方法，工业机器人的末端工具为固接在机器人第六自由度的刚体，其特征是以通用NC代码为工业机器人的工作语言，将工业机器人的末端工具作为虚拟刀具，在编程坐标系下，设置五个点O、P、Q、S、T来描述虚拟刀具的基本特征，点O设置在编程坐标系的原点，点P设置在编程坐标系的Z轴上，其他三个点Q、S、T根据末端工具的具体特征来设置，其中，设置Q点要使得刀具安装后Q点与机器人手腕中心重合，且直线QS与直线ST互相垂直；将每条NC代码的数据通过虚拟刀具的五点由编程坐标系变换到机器人坐标系，进行轨迹规划，并根据所述变换到机器人坐标系后的五个点得到机器人手腕中心点的位置和手腕姿态，从而获得机器人的逆解参数，进行机器人逆运动学求解。

2.根据权利要求1所述的一种基于NC代码的工业机器人轨迹规划方法，其特征是当虚拟刀具的五点变换到机器人坐标系后，利用三次五次插值法进行轨迹规划，所述三次五次插值法为：先取路径上的四个点O、P、Q、S进行三次插值获得中间两点P、Q的速度、加速度信息，再取P、Q、S、T四个点进行三次插值获得前两点P、Q的速度、加速度信息，将两次获取的P、Q点的速度、加速度取平均值，作为P、Q点最终的速度和加速度，加上P、Q两点的位移信息，共6个条件，插值一条五次曲线，以此保证控制点的位移以及控制点导数连续，即在平滑性的基础上获得精确的数据点。

3.根据权利要求1或2所述的一种基于NC代码的工业机器人轨迹规划方法，其特征是工业机器人为五轴机床时，包括以下步骤：

S1)获得NC代码：先设置后处理器后生产代码，设转动轴为B、C轴，后处理器中设置第四轴、第五轴偏置均为0，其基本格式为：

N**G**X**Y**Z**B**C**F**S**

其中，N为行号，G为插补方式，X、Y、Z为编程坐标系中，工件表面上一点的坐标，B、C为B、C轴转角，F为进给速度，S为主轴转速；

S2)将虚拟刀具的5个点分别绕Y轴和Z轴旋转相应的角度θ，式(1)是绕Y轴旋转的旋转矩阵，式(2)是某点(x，y，z)绕Y轴旋转变换得到(x′，y′，z′)：

$\mathrm{Rot}(y,\mathrm{\θ})=\left[\begin{array}{ccc}\mathrm{c\θ}& 0& \mathrm{s\θ}\\ 0& 1& 0\\ -\mathrm{s\θ}& 0& \mathrm{c\θ}\end{array}\right]---\left(1\right)$

$\left[\begin{array}{c}{x}^{\′}\\ y^{\′}\\ z^{\′}\end{array}\right]=\mathrm{Rot}(y,\mathrm{\θ})\left[\begin{array}{c}x\\ y\\ z\end{array}\right]---\left(2\right)$

其中，sθ＝sinθ，cθ＝cosθ；

S3)(x′，y′，z′)加上刀尖点，即工件表面点的位置坐标(X，Y，Z)，变换后得到在编程坐标系{Piece}下虚拟刀具的5个点坐标(x_P，y_P，x_P)，如式(3)：

$\left[\begin{array}{c}{x}_P\\ y_P\\ z_P\end{array}\right]=\left[\begin{array}{c}{x}^{\′}\\ y^{\′}\\ z^{\′}\end{array}\right]+\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]---\left(3\right);$

S4)经过机器人对工件进行标定，得到编程坐标系{Piece}和机器人坐标系{Base}之间的变换关系，求出{Base}坐标系下虚拟刀具的5个点坐标(x_B，y_B，z_B)，如式(4)所示：

$\left[\begin{array}{c}{x}_B\\ y_B\\ z_B\end{array}\right]=R_P^B\left[\begin{array}{c}{x}_P\\ y_P\\ z_P\end{array}\right]+P_{P_0}^B---\left(4\right)$

其中，

是{Piece}坐标系相对于{Base}坐标系的旋转矩阵，

为{Piece}坐标系原点在{Base}坐标系中的描述；

S5)在{Base}坐标系下，利用三次五次插值法进行轨迹规划：

CAM后处理器处理出来的NC代码具有连续的数据点，取作业过程中连续4个数据通过三次插值法进行轨迹插值，以相邻点的弦长l为横坐标 $l=\sqrt{{(x_2-x_1)}^2+{(y_2-y_1)}^2+{(z_2-z_1)}^2},$ 分别以X(l)、Y(l)、Z(l)、B(l)、C(l)为纵坐标进行计算，

对于X(l)，三次曲线分别设为：

x₁(l)＝a₁*l³+b₁*l²+c₁*l+d₁

(5)

x₂(l)＝a₂*l³+b₂*l²+c₂*l+d₂

将4个插值点数据代入：

X(l₁)＝a₁*l₁ ³+b₁*l₁ ²+c₁*l₁+d₁

X(l₂)＝a₁*l₂ ³+b₁*l₂ ²+c₁*l₂+d₁(6)

X(l₃)＝a₁*l₃ ³+b₁*l₃ ²+c₁*l₃+d₁

X(l₄)＝a₁*l₄ ³+b₁*l₄ ²+c₁*l₄+d₁

4个方程4个未知数，则参数a₁、b₁、c₁、d₁可求，软件编程时写成矩阵形式，求出参数a₁、b₁、c₁、d₁，代入原方程，则可求出P、Q两点的一阶导数、二阶导数：

X′_P1(l₂)＝3a₁*l₂ ²+2b₁*l₂+c₁

X′_Q1(l₃)＝3a₁*l₃ ²+2b₁*l₃+c₁(7)

X″_P1(l₂)＝6a₁*l₂+2b₁

X″_Q1(l₃)＝6a₁*l₃+2b₁

同理可得，从x₂(l)获得的P、Q两点的速度加速度为：

X′_P2(l₂)＝3a₂*l₂ ²+2b₂*l₂+c₂

X′X₂(l₃)＝3a₂*l₃ ²+2b₂*l₃+C₂(8)

X″_P2(l₂)＝6a₂*l₂+2b₂

X″_Q2(l₃)＝6a₂*l₃+2b₂

取两次P、Q的速度加速度的平均值作为P、Q两点最终的速度和加速度，得：

X′(l₂)＝(X′_P1(l₂)+X′_P2(l₂))/2

X′(l₃)＝(X′_Q1(l₃)+X′_Q2(l₃))/2(9)

X″(l₂)＝(X″_P1(l₂)+X″_P2(l₂))/2

X″(l₃)＝(X″_Q1(l₃)+X″_Q2(l₃))/2

至此，有了P、Q点共6个条件，设五次方程：

将P、Q点6个条件代入：

X′(l₂)＝f′_X(l₂)

X″(l₂)＝f″_X(l₂)(11)

X′(l₃)＝f′_X(l₃)

X″(l₃)＝f″_X(l₃)

6个方程6个未知数，同理可求出α、β、γ、μ、和ζ每个参数都是l₂l₃的表达式，软件编程时写成矩阵形式求解；

至此，求出六个系数后可用等步长法进行插值进算：

Y(l)、Z(l)、B(l)、C(l)为纵坐标的计算方法与X(l)计算方法相同，全部计算结束后，NC代码上的信息转化到虚拟刀具上五个点在{Base}坐标系下的坐标；

S6)根据{Base}坐标系下虚拟刀具的5个点坐标，求出机器人手腕中心Q点位姿：Q点坐标就是手腕中心点的位置坐标；Q点和S点确定了接近矢量

S点和T点决定了方向矢量

法线矢量

从而三个矢量确定手腕中心点的姿态；

S7)根据S6)中的逆解参数，即S6)中的Q点坐标和三个矢量进行机器人逆运动学求解。

Images