方式描述机器人工具位姿，方便描述工具的位姿变化，并且实现机器人的平滑 运动控制。该方法实现流程包括：1、工具坐标系标定；2、确定程序段首末端点处工具坐 标系的框架形式；3、进行工具位姿的四元数插值计算；4、计算插值后工具坐标系的框架形 式；5、将工具坐标系中框架形式转化为末端执行器坐标系的框架形式，进行机器人逆向运 动学求解，确定各关节位置值。
[0043] 1.工具坐标系标定
[0044] 工具坐标系标定是工业机器人控制器必需具备的一项功能。由工具中屯、点位置标 定和工具坐标系姿态标定2部分组成。其中位置标定可根据需要选择标定点数，包括3点 标定到7点标定一共5种方式。如图1所示，采用5点标定法进行位置标定。姿态标定包 括默认方向、Z方向标定和Z/X方向标定3种方式。
[0045] 所述的工具坐标系标定就是确定工具坐标系{T}相对于末端执行器坐标系巧}的 变换矩阵，机器人的坐标系可W分成四类：机器人基坐标系巧}、末端执行器坐标系巧}、工 具坐标系{T}、用户坐标系扣}。机器人基坐标系{B}是一个不可设置的缺省的坐标系，固 结在机器人机座上，它是机器人运动学分析的基本参考系。同时，机器人基坐标系{B}还可 用作通用坐标系，作为用户坐标系的参考位置。末端执行器坐标系巧}，即与机器人最后一 个连杆固结的坐标系，原点位于机器人末端法兰盘中屯、。工具坐标系{T}用来定义工具的 位置及工具的姿态。用户坐标系{U}是用户在工作空间中自定义的坐标系，是程序中记录 的所有位置的参考坐标系。
[0046] 工具坐标系{T}用来定义工具的位置及姿态，末端执行器坐标系巧}与工具坐标 系{T}如图2所示，图中XF-YF-ZF为末端执行器坐标系巧} ,XT-YT-ZT为工具坐标系{T}。
[0047] 2.工具位姿的框架计算
[0048] 所述的程序段首末端点处工具坐标系的框架形式，就是程序段首末端点处工具坐 标系{T}相对于基坐标系{B}的原点位置坐标及工具坐标系{T}各坐标轴单位方向矢量的 表示。它由工具坐标系{T}相对于末端执行器坐标系巧}和末端执行器坐标系巧}相对于 基坐标系{B}的复合变换得到。其中工具坐标系{T}相对于末端执行器坐标系巧}的变换 矩阵由工具坐标系标定得到，末端执行器坐标系巧}相对于基坐标系巧}的变换矩阵是由 机器人运动学正解得到。
[0049] 坐标系之间的关系用齐次变换矩阵来表示。例如末端执行器坐标系巧}到基坐标 系脚的位姿关系可W用矩阵款表示：
[0050] (I) 0 0 0 I
[0051] f及为末端执行器坐标系巧}相对于基坐标系{B}的旋转矩阵，它可W分块为3个 方位矢量扣，私，Ia，分别表示巧}的3个单位主矢量相对于巧}的方向余弦。Bpe。为 末端执行器坐标系巧}的原点相对于基坐标系巧}的位置矢量。可由机器人正向运动 学得到。 阳 05引 Be 巧 T=BjT C 2：)
[0053] 工具坐标系{T}相对于基坐标系巧}的位姿，可由工具坐标系{T}相对于末端执 行器坐标系巧}和末端执行器坐标系巧}相对于基坐标系巧}的复合变换得到。
[0054] 3.工具位姿的四元数插值计算
[0055] 所述的工具位姿的四元数插值计算，就是根据程序段首末端点处的工具坐标系框 架，确定转换矩阵，然后将转换矩阵由式（4)转化为四元数表示形式，利用四元数的几何特 征，将四元数表示形式根据式（5)转换为相对于基坐标系的单位法矢量v(x，y，z)及绕单位 法矢量的旋转角度angle。运样就将对工具姿态的插值计算转化为对旋转角度的插值。
[0056] 根据公式（2)计算程序段起始点处工具姿态蛋r，同理根据程序段终点各关节位置 值，确定终点处工具姿态的框架描述根据程序段首末点的框架表示，计算变换矩阵
(3)
[0058] 其中，钉'为程序段起始点处工具位姿的框架描述，许为程序段终点处工具位姿 的框架描述，Mii为变换矩阵中i行j列处的元素值。
[0059] 将（3)中的变换矩阵转化成四元数表示：
(斗） 阳OW] 其中，Mu为式（3)变换矩阵中i行j列处的元素值，w，x，y，z为四元数（W, (X，y, Z)) 中各元素的数值。
[0062] 根据四元数的几何特征，四元数描述的几何示意图如图3所示。图中CO为单位法 矢量，a为旋转角度。根据式（5)将四元数表示形式转换为空间单位法矢量V(x，y，z)及 绕单位法矢量的旋转角度angle。对所求得的角度进行插补，确定各插补点的角度值。 (5) 阳064] 其中，V (X，y, z)为空间单位法矢量，atan2 (sh, w)为根据sh及W求取反正切值的 数学函数，angle为绕单位法矢量的旋转角度，等式右边的x，y，z为式（4)中所求得的四元 数中的对应元素值。
[00化]4计算插值后工具坐标系的框架形式
[0066] 所述的计算插值后工具坐标系的框架形式，就是计算插值后工具坐标系相对于基 坐标系的原点位置坐标及工具坐标系各坐标轴单位方向矢量的表示。首先将相对于基坐标 系的单位法矢量v(x，y，z)及绕单位法矢量的旋转角度插补值转化为四元数表示形式。然 后由程序段初始点处{T}相对于巧}的框架表示与式（7)所得的转换矩阵进行复合变换， 得到插补点处的工具坐标系框架表示形式。 阳067] 因为四元数看做一个标量和一个3D向量的组合。实部W表示标量，虚部表示向量 标记为V，或S个单独的分量（X，y, Z)。所W四元数可W记为[W, V]或[W，（X，y, X)]。对四 元数中W与旋转角度有关，V与旋转轴有关。根据式（6)计算四元数表示形式
[0068] q = (wl, (xl,yl,zD)
[0069] = [cos (angle/2), (sin(angle/2)*x, sin(angle/2)*y, sin(angle/2)*z)] (6) 阳070] 其中，q为所求四元数的表示形式（wl, (xl, yl, zl))，angle为式（5)中所求得的 绕单位法矢量的旋转角度，X，y，Z为式巧）中等式左边V (X，y，z)中的对应元素值；
[0071] 将式化）中所计算的四元素 q按式（7)进行单位化，确定单位化四元数ql为 (W, (X, y, Z))，其中等式右边的wl，xl，yl，Zl由式（6)确定，ql中的X，y，Z为式（7)对应 等式左边V. X，V. y，V. Z的对应值，ql中的W与式化）中的Wl相等。
(7)
[0073] 由程序段初始点处工具坐标系{T}相对于末端执行器坐标系巧}的框架表示与式 (8)所得的变换矩阵Rm进行复合变换，得到插补点处的工具坐标系框架表示形式：
[007引其中，W，X，y，Z为式（7)所求的ql四元数（W, (X, y, Z))中各元素的数值。
[0076] 由式（9)确定工具坐标系的框架表示形式的姿态部分为： (9)
[007引其中，Rm为由式做中所确定的变换矩阵，V XX，V Vxz为程序段初始点框架中X 轴单位方向矢量（Vw Vw VJ中的元素值，Vyy, Vyy, Vy,为程序段初始点框架中y轴单位方 向矢量（Vyx，Vyy, Vyz)中的元素值，Vzx, Vzy, Vzz为程序段初始点框架中Z轴单位方向矢量 (Vzx，Vz" Vzz)中的元素值。
[00巧]结合由插补计算所得的位置值BpT。，生成相对于基坐标系的工具坐标系的框架表 示形式为：
(10)
[0081] 其中，訂?插补点处工具坐标系相对于基坐标系的变换矩阵，BpT。插补点处工具坐 标系原点在基