机器人空间三点圆弧算法的研究与实现

曾辉 柳贺

摘 要：本文介绍了一种针对机器人空间圆弧的插补方法。机器人TCP经历的空间任意三点,求出空间圆弧的圆心和半径，继而推导出机器人控制系统中任意空间圆弧的实现方法，该算法不仅理论上可使所有插补点均在圆弧上，而且采用了矢量算法，避免了插补方向和过象限的判断。

关键词：机器人；空间圆弧；插补；矢量

中图分类号：TP242 文献标识码：A 引言

机器人一般应用于比较恶劣或人难于企及的环境以替代劳动者完成必要的任务，因而机器人的编程，很多情况下是采用示教完成的。示教过程包括把机器人移动到几个所要求的目标点并把这些点的位置记录在存储器中，然后定义经过这些点的曲线轨迹及速度。

当曲线轨迹为圆弧时，除了示教圆弧起点(机器人当前点) 和终点外，至少还应知道圆心或圆弧上一中间点。显然，根据现场的应用经验，机器人终端TCP的轨迹圆弧通常由示教的圆弧起点、中间点及圆弧终点决定，而这三点所决定的平面通常不一定平行于某一坐标平面，因而需要研究空间任意三点圆弧的插补算法。

本研究以上述需求为出发点，基于机器人TCP经历的空间任意三点，推导出了机器人控制系统中任意空间圆弧的实现方法，并将该算法转化为机器人语言进行现场应用。

1 由空间任意三点求圆弧的圆心和半径

已知空间任意三点分别为圆弧起点P ( Xp ,Yp , Zp ) 、中间点 Q ( Xq , Yq , Zq )和圆弧终点R( Xr , Yr , Zr ) ,如图1所示。设圆心为C( Xc ,Yc , Zc) ,半径为r ,则有：

（1）

设坐标系 O-X Y Z 上各坐标轴的单位坐标矢量分别为i , j , k ,则有：

（2）

P、Q、R确定的平面I的法向量

（3）

则在平面I内垂直于的向量为：

（4）

则在平面I内过中点N1（XN1，YN1，ZN1），和垂直的直线L1为：

（5）

则在平面I内垂直于的向量为：

（6）

则在平面I内过中点N2（XN2，YN2，ZN2），和垂直的直线L2为：

（7）

直线L1和直线L2的交点即为所求圆弧的圆心C( Xc ,Yc , Zc)，由CP、CQ、CR任一求出半径r。

在求解过程中应注意平行于任一坐标平面的圆弧的处理，若L1平行于X平面，则N1i=0，若L2平行于X、Y平面，则N2i、N2j均等于0。在这些情况下都需要程序特殊处理。如图2所示。

2插补算法

空间给定了起点、终点和一个中间点的空间三点圆弧的走向是确定的,因此无需用顺时针和逆时针来区分。事实上，用表示空间三点圆弧所在平面的法矢量 n ,则从 n 的正方向看,从 P 到 Q 到R 的圆弧始终是逆时针圆弧。机器人的控制系统读入此段程序后,首先求出圆心和半径,然后再计算插补点的坐标。

在平面I内建立新的二维坐标系XOY。以CP为新的Y轴，以nXCP为X轴，对X、Y分别进行单位化，就可得到新的坐标系O-XY到原坐标系O-XYZ的转换矩阵。

（7）

（8）

（9）

（10）

则转换矩阵

（11）

这样就把空间圆弧算法转化为平面I的圆弧算法。

平面圆弧上的任一点M的O-XY坐标系下坐标（Xm，Ym）都能转化为空间的O-XYZ坐标系的坐标（Xm，Ym，Zm）。

（12）

3 终点判别

圆弧插补的终点判别只需算出圆心角θ和步距角δ,以两者的商作为插补次数。

圆心角θ的计算则要考虑如图3所示的 (圆弧 PQR )和 (圆弧 PQR )两种情况。

当时,

当时，

（14）

从图3可以看出,当时，矢量

与圆弧所在平面的法矢量方向相同；

当时，矢量与方向相反。因而可用公式：

（15）

的正负来判断的范围,当时, ；当时，。式中为矢量在各坐标轴方向上的分量。

算出δ和θ后，插补次数(不包括 P点)。

4仿真分析

在空间任意给定三点，首先算出圆心，然后根据上述算法画出圆弧。

时的圆弧如图4所示。

时的圆弧如图5所示。

5机器人实现

对于6自由度机器人来说，通过圆弧的插补算法实时获取圆弧的插补位置,然后经过机器人逆解，获得机器人的关节角度，驱动各轴电机运动到相应位置，到达TCP所要求的位置和方位，改方法成功应用到自己开发的ER-165点焊与ER6弧焊机器人，经激光跟踪仪测定，跟随误差都能控制在+-0.02mm以内。

参考文献

[1] 孙树栋.工业机器人技术基础[M] 西安：西安工业大学出版社，2006.

[2] 杨桥.奇瑞QH-165点焊机器人焊接系统浅析[J] .机器人技术与应用2010，3：10-11.

[3] 熊有伦 . 机器人技术基础 [ M ] . 武 汉 : 华 中理工大学 出 版社 , 1996.

[4]叶伯生 . 计 算机 数 控系 统原 理、 编 程 与操 作 [ M ] . 武 汉 : 华中理工大学出版社 , 1999.

[5] 秦开怀 , 金建新 , 宾 鸿赞 . CN C 系统 中任 意三 维椭 圆 弧的高速插补新方法 [ J] . 华 中理工 大学学 报 , 1992, 20( 6) : 7 - 11.