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];
已知RPY角位姿表达式,求齐次坐标变换矩阵位姿表达式,也称RPY角正解
最新推荐文章于 2024-02-05 09:41:10 发布