一种六轴机器人末端跟随目标物实时位姿的计算方法与流程

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

背景技术：

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

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

技术实现要素：

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

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

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

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轴的向量分量nz,h应为0，即：

nz,h＝nz'*nx,r+sz'*ny,r+az'*nz,r+pz'*0＝0

根据上述等式求出末端轴的偏差角，再通过机器人逆运动学可得到各关节应转动的角度θ1、θ2、θ3、θ4、θ5、θ6，从而得到六轴需要转动的角度，便可以使得机器人末端水平轴始终平行于工作台；

s13、重复s3，通过不断更新位姿，保持实时的追踪。

所述s4中计算机器人末端坐标系的齐次变换矩b的方法为：相机在末端坐标系的安装位置为(a，b，c)，相机采集到目标物相对于相机的位置坐标(x，y，z)，那么目标物在末端坐标系的位置坐标为(a+x，b+y，c+z)，目标物坐标系相对于机器人末端坐标系先绕x轴旋转90度，再绕z轴旋转90度，计算目标物坐标系相对于机器人末端坐标系的变换矩阵

所述s5中求解目标物相对于目标物自身的初始坐标系的齐次变换矩阵c的方法为：设目标物在坐标系下的滚动角为r、俯仰角为p、偏航角为y，计算其旋转矩阵为

所述s8中机器人末端坐标系的变换矩阵记b'的求解方法为：设始终保持的距离为z，坐标系先绕x轴旋转90度，再绕z轴旋转90度，计算目标物相对于机器人末端的转换矩阵为

所述s9中目标物相对于目标物自身坐标系的旋转矩阵c'的求解方法为：由于机器人末端正对着目标物，因此目标物在坐标系下滚动角、俯仰角、偏航角都为0，即目标物相对于目标物自身坐标系的旋转矩阵为单位矩阵

所述s11中机器人末端相对于世界坐标系新的位姿a'的求解方法为：根据t＝a*b*c和t＝a'*b'*c'两个等式，可得到新的状态下机械臂末端相对于世界坐标系的矩阵a'。

所述s12中偏差角的计算方法为：β＝-arctan(nz'/sz')。

所述s12中六轴需要转动的角度的计算方法为：在末端轴逆解的基础上再减去偏差角，得出六轴需要转动的角度分别为θ1、θ2、θ3、θ4、θ5、θ6－β。

本发明与现有技术相比，具有的有益效果是：

本发明实现了六轴机器人末端实时跟随目标物，能够实时的求解追踪与机器人没有任何连接关系的目标物，并推算出机器人末端应该调整的位姿；而且通过该算法使得机器人末端始终与工作台保持水平。

附图说明

图1为本发明的位置坐标示意图；

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

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

s1、如图1所示，以六轴机器人第一轴与工作台的交点o0为原点建立世界坐标系x0y0z0。

s2、在各个关节轴建立坐标系、机器人末端建立坐标系x6y6z6以及在目标物建立坐标系xmymzm。

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

s4、相机在末端坐标系的安装位置为(a，b，c)，相机采集到目标物相对于相机的位置坐标(x，y，z)，那么目标物在末端坐标系的位置坐标为(a+x，b+y，c+z)，目标物坐标系相对于机器人末端坐标系先绕x轴旋转90度，再绕z轴旋转90度，计算目标物坐标系相对于机器人末端坐标系的变换矩阵

s5、设目标物在坐标系下的滚动角为r、俯仰角为p、偏航角为y，计算目标物相对于目标物自身的初始坐标系的齐次变换矩

s6、求解当前目标物在世界坐标系的位姿：t＝a*b*c。

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

s8、设始终保持的距离为z，坐标系先绕x轴旋转90度，再绕z轴旋转90度，计算目标物相对于机器人末端的转换矩阵为

s9、由于机器人末端正对着目标物，因此目标物在坐标系下滚动角、俯仰角、偏航角都为0，即目标物相对于目标物自身坐标系的旋转矩阵为单位矩阵

s10、求解目标物在世界坐标系的位姿t＝a'*b'*c'。

s11、求解机器人末端相对于世界坐标系新的位姿a'，根据t＝a*b*c和t＝a'*b'*c'两个等式，可得到新的状态下机械臂末端相对于世界坐标系的矩阵a'。

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

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

nz,h＝nz'*nx,r+sz'*ny,r+az'*nz,r+pz'*0＝0

根据上述等式求出末端轴的偏差角，偏差角的计算方法为：β＝-arctan(nz'/sz')，再通过机器人逆运动学可得到各关节应转动的角度θ1、θ2、θ3、θ4、θ5、θ6，在末端轴逆解的基础上再减去偏差角，得出六轴需要转动的角度分别为θ1、θ2、θ3、θ4、θ5、θ6－β。，便可以使得机器人末端水平轴始终平行于工作台。

s13、重复s3，通过不断更新位姿，保持实时的追踪。

上面仅对本发明的较佳实施例作了详细说明，但是本发明并不限于上述实施例，在本领域普通技术人员所具备的知识范围内，还可以在不脱离本发明宗旨的前提下作出各种变化，各种变化均应包含在本发明的保护范围之内。