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

权利要求书

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

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

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

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

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

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

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

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

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

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

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

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

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

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

2.根据权利要求1所述的一种六轴机器人末端跟随目标物实时位姿的计算方法，其特征在于：所述S4中计算机器人末端坐标系的齐次变换矩B的方法为：相机在末端坐标系的安装位置为(a，b，c)，相机采集到目标物相对于相机的位置坐标(x，y，z)，那么目标物在末端坐标系的位置坐标为(a+x，b+y，c+z)，目标物坐标系相对于机器人末端坐标系先绕x轴旋转90度，再绕z轴旋转90度，计算目标物坐标系相对于机器人末端坐标系的变换矩阵

3.根据权利要求1所述的一种六轴机器人末端跟随目标物实时位姿的计算方法，其特征在于：所述S5中求解目标物相对于目标物自身的初始坐标系的齐次变换矩阵C的方法为：设目标物在坐标系下的滚动角为R、俯仰角为P、偏航角为Y，那么计算其旋转矩阵为

4.根据权利要求1所述的一种六轴机器人末端跟随目标物实时位姿的计算方法，其特征在于：所述S8中机器人末端坐标系的变换矩阵记B'的求解方法为：设始终保持的距离为Z，坐标系先绕x轴旋转90度，再绕z轴旋转90度，计算目标物相对于机器人末端的转换矩阵为

5.根据权利要求1所述的一种六轴机器人末端跟随目标物实时位姿的计算方法，其特征在于：所述S11中机器人末端相对于世界坐标系新的位姿A'的求解方法为：根据T＝A*B*C和T＝A'*B'*C'两个等式，可得到新的状态下机械臂末端相对于世界坐标系的矩阵A'。