rpy角与旋转矩阵之间的转换(附完整代码)

  根据绕轴旋转的次序不同，易知姿态的rpy(roll, pitch, yaw)表示总共有12种，分别为：XYZ, XZY, XYX, XZX; YXZ, YZX, YXY, YZY; ZXY, ZYX, ZXZ, ZYZ。同样的，姿态的欧拉角表示也有12种。在实际应用中，没必要掌握所有情况，只需认准一种，能够实现rpy角与旋转矩阵之间的转换即可。姿态的rpy表示中，ZYX最为常见，下面推导该情况下，rpy角与旋转矩阵之间的转换。

一、 rpy角转换为旋转矩阵

  ZYX rpy角转换为旋转矩阵：
$$R=Rotz(c)\ast Roty(b)\ast Rotx(a)\text{\hspace{1em}(1)}$$
  符号表达式MATLAB代码:

clc
clear

syms a b c real;

rotx_a = [sym(1), sym(0), sym(0)
        sym(0), cos(a), -sin(a)
        sym(0), sin(a), cos(a)]
    
roty_b = [cos(b), sym(0), sin(b)
        sym(0), sym(1), sym(0)
        -sin(b), sym(0), cos(b)]      
 
rotz_c = [cos(c), -sin(c), sym(0)
        sin(c), cos(c), sym(0)
        sym(0), sym(0), sym(1)]

R = rotz_c * roty_b * rotx_a


b = sym(-pi/2);
simplify(eval(R))

b = sym(pi/2);
simplify(eval(R))

二、 旋转矩阵转换为rpy角

  设已知旋转矩阵:
$$R=\left[\begin{array}{ccc}{r}_{11}& r_{12}& r_{13}\\ r_{21}& r_{22}& r_{23}\\ r_{31}& r_{32}& r_{33}\end{array}\right]\text{\hspace{1em}(2)}$$
  设rpy角$a,b,c\in [-\pi ,\pi ]$，根据旋转矩阵关于rpy角$a,b,c$的解析表达式，以最简单一项($r_{31}$)为突破口。
  1)若$r_{31}=1$，则$b=-\pi /2$，此时有：
$$R=\left[\begin{array}{ccc}0& -sin(a+c)& -cos(a+c)\\ 0& cos(a+c)& -sin(a+c)\\ 1& 0& 0\end{array}\right]\text{\hspace{1em}(3)}$$
  姿态奇异，无法计算$a,c$，只能计算$a+c$。
  取$a=0$，则有：
$$\{\begin{array}{l}{r}_{12}=-sin(c)\\ r_{13}=-cos(c)\end{array}\text{\hspace{1em}(4)}$$
  故有：
$$c=atan2(-r_{12},-r_{13})\text{\hspace{1em}(5)}$$
  2)若$r_{31}=-1$，则$b=\pi /2$，此时有：
$$R=\left[\begin{array}{ccc}0& sin(a-c)& cos(a-c)\\ 0& cos(a-c)& -sin(a-c)\\ -1& 0& 0\end{array}\right]\text{\hspace{1em}(6)}$$
  姿态奇异，无法计算$a,c$，只能计算$a-c$。
  取$a=0$，则有：
$$\{\begin{array}{l}{r}_{12}=sin(-c)=-sin(c)\\ r_{13}=cos(-c)=cos(c)\end{array}\text{\hspace{1em}(7)}$$
  故有：
$$c=atan2(-r_{12},r_{13})\text{\hspace{1em}(8)}$$
  3)若$r_{31}\ne \pm 1$，则$b\ne \pm \pi /2$，$cos(b)\ne 0$。
  $a,c$的一个解：
$$\{\begin{array}{l}a=atan2(r_{32},r_{33})\\ c=atan2(r_{21},r_{11})\end{array}\text{\hspace{1em}(9)}$$
  $a,c$的另一个解：
$$\{\begin{array}{l}a=atan2(-r_{32},-r_{33})\\ c=atan2(-r_{21},-r_{11})\end{array}\text{\hspace{1em}(10)}$$
  若$|cos(c)|>|sin(c)|$,
$$b=atan2(-r_{31},r_{11}/cos(c))\text{\hspace{1em}(11)}$$
  否则，
$$b=atan2(-r_{31},r_{21}/sin(c))\text{\hspace{1em}(12)}$$

%{
Function: rpy2rotm
Description: rpy转旋转矩阵R， R= rotz(c) * roty(b) * rotx(a)
Input: rpy角度(单位rad)
Output: 旋转矩阵R
Author: Marc Pony(marc_pony@163.com)
%}
function R = rpy2rotm(rpy)

a = rpy(1);
b = rpy(2);
c = rpy(3);

sinA = sin(a);
cosA = cos(a);
sinB = sin(b);
cosB = cos(b);
sinC = sin(c);
cosC = cos(c);

R = [cosB*cosC,  cosC*sinA*sinB - cosA*sinC,  sinA*sinC + cosA*cosC*sinB
    cosB*sinC, cosA*cosC + sinA*sinB*sinC, cosA*sinB*sinC - cosC*sinA
    -sinB, cosB*sinA,  cosA*cosB];

end

%{
Function: rotm2rpy
Description: 旋转矩阵R转rpy， R= rotz(c) * roty(b) * rotx(a)
Input: 旋转矩阵R
Output: rpy角度(单位rad)
Author: Marc Pony(marc_pony@163.com)
%}
function rpy = rotm2rpy( R )

if abs(R(3 ,1) - 1.0) < 1.0e-15   % singularity
    a = 0.0;
    b = -pi / 2.0;
    c = atan2(-R(1, 2), -R(1, 3));
elseif abs(R(3, 1) + 1.0) < 1.0e-15   % singularity
    a = 0.0;
    b = pi / 2.0;
    c = -atan2(R(1, 2), R(1, 3));
else
    a = atan2(R(3, 2), R(3, 3));
    c = atan2(R(2, 1), R(1, 1));
    %     a = atan2(-R(3, 2), -R(3, 3));  %a另一个解
    %     c = atan2(-R(2, 1), -R(1, 1));  %c另一个解
    cosC = cos(c);
    sinC = sin(c);
    
    if abs(cosC) > abs(sinC)
        b = atan2(-R(3, 1), R(1, 1) / cosC);
    else
        b = atan2(-R(3, 1), R(2, 1) / sinC);
    end
end

rpy = [a, b, c];

end

clc
clear

n = 0;
while 1
    
    rpy = -pi + 2*pi * rand(1, 3);
    %rpy(2) = -pi/2; %奇异：+-pi
    R = rotz(rpy(3)) * roty(rpy(2)) * rotx(rpy(1));
    
    rpy1 = rotm2rpy(R);
    R1 = rpy2rotm(rpy1);
    
    rpy2 = rotm2rpy(R1);
    R2 = rpy2rotm(rpy2);
    
    if sum(sum(abs(R - R1))) > 1.0e-8 || sum(sum(abs(R - R2))) > 1.0e-8 || sum(sum(abs(R1 - R2))) > 1.0e-8
        break;
    end
    
    %    同样旋转矩阵R可能对应不同的rpy角度
    %     if sum(abs(rpy - rpy1)) > 1.0e-8 || sum(abs(rpy - rpy2)) > 1.0e-8 || sum(abs(rpy1 - rpy2)) > 1.0e-8
    %         break;
    %     end
    
    n = n + 1
end

三、 小结

  (1)旋转矩阵中，当$r_{31}=1或-1$时，姿态奇异，只能计算$a+c或a-c$的值，此时通常的做法是令$a=0$。
  (2)当$r_{31}\ne \pm 1$时，rpy角有两组解。类似地，当旋转矩阵用轴角表示时，有两组解：绕轴$k$旋转$\theta$角度与绕轴$-k$旋转$-\theta$角度；当旋转矩阵用单位四元数表示时，有两组解：单位四元数$q$与单位四元数的相反数$-q$。因而，在姿态的单位四元数SLERP插补中，有必要选取较短的“姿态路径”,使得机器人姿态动作看起来柔顺一些。
  (3)当$r_{31}=1或-1$时，一个旋转矩阵对应无穷多组rpy角。当$r_{31}\ne \pm 1$时，一个旋转矩阵对应两组rpy角。由于通常姿态插补为轴角插补或单位四元数SLERP插补，rpy角只作界面显示，在实际编程中，当姿态不奇异时，只需考虑一组解即可。机器人末端姿态的旋转矩阵R通过正解得到，旋转矩阵转换成rpy显示在界面上，上位机下发rpy角后，将rpy角转换成旋转矩阵R，再用轴角插补或单位四元数SLERP插补。也就是说，只要保证程序中的rpy角与旋转矩阵之间的变换关系是一一对应就可以，不需要也没必要去讨论选取哪一组rpy角。
  姿态插补中，RPY与旋转矩阵R之间转换的一个坑:
  轴角插补或单位四元数插补后，将轴角或单位四元数转换为旋转矩阵R，再把位置插补的位置XYZ合起来，写成一个4*4的齐次变换矩阵，作为机器人逆解的位姿输入。
  有时候，为了节省内存空间，将旋转矩阵R转换为RPY角，再把位置插补的位置XYZ合起来，写成一个6维向量，作为机器人逆解的位姿输入(注意:做逆解时需要将RPY转换为旋转矩阵R)。
  如果轴角或单位四元数转换为旋转矩阵R时，均为单精度浮点运算，导致旋转矩阵R丢失了些精度。由于RPY的姿态表示方法存在姿态奇异(万向节锁)，当旋转矩阵R转换为RPY角时，在奇异点附近计算的 RPY角误差比较大，从而导致RPY转换而成的旋转矩阵R跳变。
  如何避坑：
  (1)轴角/单位四元数 -> 旋转矩阵 -> RPY -> 旋转矩阵，均采用双精度浮点数运算。
  (2)旋转矩阵转换为RPY时，判断奇异点的容差值尽可能小。文章来源地址https://uudwc.com/A/B60W