第二章 （已实现）基于Python语音控制（讯飞星火）UR机器人精确动作之控制UR机器人精确运动

系列文章

第一章 （已实现）基于Python语音控制（讯飞星火）UR机器人精确动作之获取UR机器人位置
第二章 （已实现）基于Python语音控制（讯飞星火）UR机器人精确动作之控制UR机器人精确运动

---

前言

前一章已经实现python通过Socket通讯实时获取当前TCP的位置(x,y,z,rx,ry,rz)，如何使得UR机器人按照要求运动到指定位置是本文将要介绍的。本文讲实现UR机器人基于大地坐标的X,Y,Z方向上的移动（即前后左右上下）的线性运动，以及相对工具坐标的绕X，绕Y，绕Z轴的运动。

---

一、UR机器人运动控制简介

笔者使用UR机器人时间较短，对UR机器人的运动指令不是特别熟悉，只熟悉通过Python发送MOVEL指令远程控制机器人运动到指定点。在网上查询Python中UR机器人运动控制指令，如RTDE，urx等python包或多或少出现这样那样的错误，最终笔者决定，根据python中的矩阵运算功能，通过机器人矩阵求解方式，直接算出最终机器人TCP点的目前位置来满足运动要求，此部分对于不熟悉矩阵变换的同学可能较为困难，可直接跳过，直接复制代码即可。

二、UR机器人线性运动（上下左右前后）

1.机器人XYZ方向线性运动

此部分较为简单，在当前获取TCP位置(x,y,z,rx,ry,rz)的基础上，在x,y,z相应的加减即可

代码如下（以机器人向上为例子），：

def up(u): #u为向上需要移动的距离，以毫米为单位
    pose=get_pose() #为前一章节获得机器人当前位置的函数
    x = pose[0]
    y = pose[1]
    z = pose[2]
    rx = pose[3]
    ry = pose[4]
    rz = pose[5]
    robot = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    robot.connect((HOST, PORT))   #socket通讯连接，HOST为机器人IP，PORT为端口号30003
    z1 = z + u / 1000 #u 以毫米为单位，最终发给机器人的是米为单位，因此需要除以1000
    command = b"movel(p[%f ,%f ,%f ,%f, %f, %f],a=0.1,v=0.2)\n" % (x, y, z1, rx, ry, rz)
    robot.send(command)
    robot.close()

2.机器人绕工具坐标X,Y,Z运动

此功能实现主要笔者在不熟悉UR机器人旋转命令，只知道如何控制机器人运行到指定点的情况下，运用矩阵算法求得最终目标点的位置实现，简单介绍下思路，看不懂的直接复制代码即可,本文绕工具坐标XYZ分两种矩阵算法，绕XY为法向量旋转法，绕z为直接旋转法。（以下机器人坐标为大地坐标）

2.1.机器人绕工具坐标运动（法向量偏移法）

算法思路：在机器人坐标下，根据目前法兰盘的法向量和最终旋转后的法向量，两者通过旋转重合，求得旋转矩阵，最终通过当前的姿态左乘这个旋转矩阵求得最终机器人的姿态。
1、在工具坐标下，法兰盘的法向量为（0,0,1）
2、在工具坐标下，法兰盘法向量乘以绕X轴的旋转矩阵(标准矩阵，可百度)获得工具坐标下的目标法向量。