MOTOMANHP6的位姿矩阵的计算,建立坐标系,确定参数,matlab计算程序,姿态矩阵逆解