已知RPY角位姿表达式，求齐次坐标变换矩阵位姿表达式，也称RPY角正解

function T=zhengjie_for_T(Q) %T为4*4矩阵，Q为1*6向量 RX=[1 0 0;0 cos(Q(4)) -sin(Q(4));0 sin(Q(4)) cos(Q(4))]; RY=[cos(Q(5)) 0 sin(Q(5));0 1 0;-sin(Q(5)) 0 cos(Q(5))]; RZ=[cos(Q(6)) -sin(Q(6)) 0;sin(Q(6)) cos(Q(6)) 0; 0 0 1]; R=RZ*RY*RX; T=[R Q(1:3)';     0 0 0 1];