已知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];