[发明专利]一种六轴机器人末端跟随目标物实时位姿的计算方法

说明书

本发明属于位姿计算技术领域，具体涉及一种六轴机器人末端跟随目标物实时位姿的计算方法，一种六轴机器人末端跟随目标物实时位姿的计算方法，包括下列步骤：通过获取运动的目标物的参数，经过位姿转换、矩阵运算可以得到目标物的实时位姿，并推算出机器人末端应该调整的位姿。本发明实现了六轴机器人末端实时跟随目标物，能够实时的求解追踪与机器人没有任何连接关系的目标物，并推算出机器人末端应该调整的位姿；而且通过该算法使得机器人末端始终与工作台保持水平。本发明用于机器人末端跟随目标物实时位姿的计算。

技术领域

本发明属于位姿计算技术领域，具体涉及一种六轴机器人末端跟随目标物实时位姿的计算方法。

背景技术

机器人的运动学是在不考虑引起运动的力和力矩的情况下，描述机械臂的运动。机器人正运动学原理根据给定的机器人关节变量确定末端执行器的位置和姿态；机器人逆运动学原理则是根据给定的末端执行器的位姿确定机器人关节变量的取值。

正运动学与逆运动学只是解决了机器人上固连在一起的连杆与关节的位姿问题，无法求解追踪与机器人没有任何连接关系的目标物。

发明内容

针对上述正运动学与逆运动学无法求解追踪与机器人没有连接关系的目标物的技术问题，本发明提供了一种六轴机器人末端跟随目标物实时位姿的计算方法。

为了解决上述技术问题，本发明采用的技术方案为：

一种六轴机器人末端跟随目标物实时位姿的计算方法，包括下列步骤：

S1、以六轴机器人第一轴与工作台的交点为原点建立世界坐标系；

S2、在各轴、机器人末端以及目标物依次建立各自坐标系；

S3、依据机器人正运动学原理得到各个关节之间的齐次变换矩阵，将矩阵相乘得到当前状态下机器人末端相对于世界坐标系的位姿

S4、计算目标物坐标系相对于机器人末端坐标系的齐次变换矩阵记为B；

S5、根据目标物的参数求解目标物相对于目标物自身的初始坐标系的齐次变换矩阵为C；

S6、求解当前目标物在世界坐标系的位姿：T＝A*B*C；

S7、为了使机器人末端始终正对着要跟随的目标物体并保持一定距离，那么新的状态下机械臂末端相对于世界坐标系的矩阵记为

S8、计算目标物坐标系相对于机器人末端坐标系的变换矩阵记为B'；

S9、计算目标物相对于目标物自身坐标系的旋转矩阵C'；

S10、求解目标物在世界坐标系的位姿T＝A'*B'*C'；

S11、求解机器人末端相对于世界坐标系新的位姿A'；

S12、为了使机器人末端的水平轴能够始终平行于工作台，将A'矩阵与绕末端轴轴线的旋转的齐次矩阵Rot相乘，得到新的矩阵记为H

当机器人末端的水平轴是x轴，那么经过矩阵A'与Rot相乘x轴在新坐标系下的z轴的向量分量n_z,H应为0，即：

n_z,H＝n_z'*n_x,R+s_z'*n_y,R+a_z'*n_z,R+p_z'*0＝0