坐标系的坐标矢量。
[0082] 5末端执行器坐标系的框架形式的确定
[0083] 利用工具坐标系标定所确定的工具坐标系{T}相对于末端执行器坐标系巧}的变 换矩阵，由式（11)确定末端执行器坐标系相对于基坐标系的框架形式。然后进行机器人逆 向运动学求解，确定各关节位置值。
[0084] 訂邱(巧Y (11)
[00化]其中，fr为式（11)所确定的插补点处工具坐标系相对于基坐标系的框架表示。 貿'为由工具坐标系标定所确定工具坐标系{T}相对于末端执行器坐标系巧}的框架表示。
【主权项】
1. 一种采用框架描述的工业机器人工具位姿控制方法，其特征在于，包括W下步骤： 工具坐标系标定：确定工具坐标系{T}相对于末端执行器坐标系巧}的变换矩阵； 确定程序段首末端点处工具坐标系的框架形式，即程序段首末端点处工具坐标系m 相对于基坐标系巧}的原点位置坐标及工具坐标系{T}各坐标轴单位方向矢量的表示 进行工具位姿的四元数插值计算：根据程序段首末端点处的工具坐标系框架，确定变 换矩阵Tc,然后将变换矩阵Tc转化为四元数表示形式，利用四元数的几何特征，将四元数 表示形式转换为相对于基坐标系的单位法矢量及绕单位法矢量的旋转角度； 计算插值后工具坐标系的框架形式，即插值后工具坐标系{T}相对于基坐标系巧}的 原点位置坐标及工具坐标系{T}各坐标轴单位方向矢量的表示； 将工具坐标系中框架形式转化为末端执行器坐标系的框架形式，进行机器人逆向运动 学求解，确定各关节位置值。2. 根据权利要求1所述的一种采用框架描述的工业机器人工具位姿控制方法，其特征 在于，所述工具坐标系{T}用于定义工具的位置及工具的姿态。3. 根据权利要求1所述的一种采用框架描述的工业机器人工具位姿控制方法，其特征 在于，所述末端执行器坐标系巧}为与机器人最后一个连杆固结的坐标系，原点位于机器 人末端法兰盘中屯、。4. 根据权利要求1所述的一种采用框架描述的工业机器人工具位姿控制方法，其特征 在于，所述程序段首末端点处工具坐标系的框架形式由工具坐标系{T}相对于末端执行器 坐标系巧}和末端执行器坐标系巧}相对于基坐标系巧}的复合变换得到； 其中，工具坐标系{T}相对于末端执行器坐标系巧}的变换矩阵由工具坐标系标定得 到 ;末端执行器坐标系巧}相对于基坐标系巧}的变换矩阵是由机器人运动学正解得到：Cl) 矩阵fr表示末端执行器坐标系巧}到基坐标系巧}的位姿关系；fi?为末端执行器坐 标系做相对于基坐标系脚的旋转矩阵，分块为3个方位矢量>，扣，iV，分别表示巧} 的3个单位主矢量相对于{B}的方向余弦；Bpe。为末端执行器坐标系巧}的原点相对于基坐 标系巧}的位置矢量。5. 根据权利要求1所述的一种采用框架描述的工业机器人工具位姿控制方法，其特征 在于，所述根据程序段首末端点处的工具坐标系框架的确定变换矩阵为： 根据公式（2)计算程序段起始点处工具位姿兴^:，同理根据程序段终点各关节位置值， 确定终点处工具位姿态的框架描化tr。根据程序段首末点的框架表示，计算变换矩阵(3) 其中，3才为程序段起始点处工具位姿的框架描述，3方为程序段终点处工具位姿的框 架描述，Ml,为变换矩阵中i行j列处的元素值。6. 根据权利要求1所述的一种采用框架描述的工业机器人工具位姿控制方法，其特征 在于，所述将变换矩阵转化为四元数表示形式：(4) 其中，Mi为式做变换矩阵中i行j列处的元素值，W，X，y，Z为四元数（W，（X，y，Z)) 中各元素的数值。7. 根据权利要求1所述的一种采用框架描述的工业机器人工具位姿控制方法，其特征 在于，所述将四元数表示形式转换为相对于基坐标系的单位法矢量及绕单位法矢量的旋转 角度：(5) 其中，v(x，y，z)为空间单位法矢量，atan2(sh，w)为根据Sh及W求取反正切值的数学 函数，angle为绕单位法矢量的旋转角度，等式右边的x，y，z为式（4)中所求得的四元数中 的对应元素值。8. 根据权利要求1所述的一种采用框架描述的工业机器人工具位姿控制方法，其特征 在于，所述计算插值后工具坐标系的框架形式，包括W下步骤： 将相对于基坐标系巧}的单位法矢量V (x，y，z)及绕单位法矢量的旋转角度插补值转 化为四元数表示形式： q = (wl, (xl, yl, zl)) 化） =[cos (angle/2), (sin(angle/2)*x, sin (angle/2)*y, sin (angle/2)*z)] 其中，q为所求四元数的表示形式（wl, (xl，yl，zl))，angle为式（5)中所求得的绕单 位法矢量的旋转角度，x，y，Z为式巧）中等式左边V (x，y，z)中的对应元素值； 将式（6)中所计算的四元素 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) 由程序段初始点处工具坐标系IT}相对于末端执行器坐标系巧}的框架表示与式（8) 所得的变换矩阵Rm进行复合变换，得到插补点处的工具坐标系框架表示形式姿态部分为：(8) 其中，W，X，y，Z为式（7)所求的ql四元数（W, (X, y, Z))中各元素的数值。 由式（9)确定工具坐标系的框架表示形式的姿态部分为：屯9) 其中，Rm为由式（8)中所确定的变换矩阵，Vu, Vyy, Vu为程序段初始点工具框架中X轴 单位方向矢量（Vw Vw VJ的矢量元素，Vyy, Vyy, Vy,为程序段初始点工具框架框架中y轴单 位方向矢量（Vyy，Vyy, VyJ中的矢量元素，Vu, 程序段初始点工具框架框架中Z轴单 位方向矢量（Vu, V,y, Vj中的矢量元素。 结合由插补计算所得的位置值BpT。，生成相对于基坐标系巧}的工具坐标系的框架表 示形式为：(10) 其中插补点处工具坐标系相对于基坐标系的变换矩阵，Bp?插补点处工具坐标系 原点在基坐标系的坐标矢量。9.根据权利要求1所述的一种采用框架描述的工业机器人工具位姿控制方法，其特征 在于，所述将工具坐标系中框架形式转化为末端执行器坐标系的框架形式，进行机器人逆 向运动学求解，确定各关节位置值，具体为： 利用工具坐标系标定所确定的工具坐标系{T}相对于末端执行器坐标系巧}的变换矩 阵，由式（11)确定末端执行器坐标系相对于基坐标系的框架形式： 社邱脚、 (U) 其中，努^为式（10)所确定的插补点处工具坐标系相对于基坐标系的框架表示。訂^为 由工具坐标系标定所确定工具坐标系{T}相对于末端执行器坐标系巧}的框架表示； 然后进行机器人逆向运动学求解，确定各关节位置值。
【专利摘要】本发明涉及一种采用框架描述的工业机器人工具位姿控制方法，该方法主要步骤包括：工具坐标系标定；确定程序段首末端点处工具坐标系的框架形式；工具位姿的四元数插值计算；计算插值后工具坐标系的框架形式；将工具坐标系中框架形式转化为末端执行器坐标系的框架形式，进行机器人逆向运动学求解，确定各关节位置值。该方法能够准确、可靠实现机器人工具位姿控制，避免了由于采用欧拉角及四元数进行姿态描述所导致的角度间插值困难、存在奇异点等问题，且该发明由于采用四元数进行插补可以实现机器人的平滑运动控制。
【IPC分类】B25J9/16
【公开号】CN105643619
【申请号】
【发明人】孙维堂, 李忠琪, 刘荫忠, 杨东升
【申请人】中国科学院沈阳计算技术研究所有限公司
【公开日】2016年6月8日
【申请日】2014年11月13日