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

Marc Pony

已于 2022-04-15 22:24:10 修改

阅读量1.1w

点赞数 25

分类专栏：机器人学文章标签： rpy角与旋转矩阵之间的转换 rpy与旋转矩阵 rpy角姿态矩阵

于 2022-03-22 22:54:53 首次发布

本文链接：https://blog.csdn.net/maple_2014/article/details/123647349

版权

机器人学专栏收录该内容

4 篇文章 70 订阅

订阅专栏

文章目录

一、 rpy角转换为旋转矩阵
二、旋转矩阵转换为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角转换为旋转矩阵：
$\tag 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角

设已知旋转矩阵:
$\left[ \begin{matrix} r_{11} & r_{12} & r_{13} \\ r_{21} &r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \\ \end{matrix} \tag 2 \right]$
设rpy角 $a,b,c\in[-\pi,\pi]$ ，根据旋转矩阵关于rpy角 $a, b, c$ 的解析表达式，以最简单一项( $r_{31}$ )为突破口。
1)若 $r_{31}=1$ ，则 $b=-\pi/2$ ，此时有：
$\left[ \begin{matrix} 0 & -sin(a+c) & -cos(a+c) \\ 0 & cos(a+c) & -sin(a+c) \\ 1 & 0 &0 \\ \end{matrix} \tag 3 \right]$
姿态奇异，无法计算 $a, c$ ，只能计算 $a + c$ 。
取 $a = 0$ ，则有：
$\begin{cases} r_{12}=-sin(c)\\ r_{13}=-cos(c)\\ \tag 4 \end{cases}$
故有：
$c=atan2(-r_{12},-r_{13}) \tag 5$
2)若 $r_{31}=-1$ ，则 $b=\pi/2$ ，此时有：
$\left[ \begin{matrix} 0 & sin(a-c) & cos(a-c) \\ 0 & cos(a-c) & -sin(a-c) \\ -1 & 0 &0 \\ \end{matrix} \tag 6 \right]$
姿态奇异，无法计算 $a, c$ ，只能计算 $a - c$ 。
取 $a = 0$ ，则有：
$\begin{cases} r_{12}=sin(-c)=-sin(c)\\ r_{13}=cos(-c)=cos(c)\\ \tag 7 \end{cases}$
故有：
$c=atan2(-r_{12},r_{13}) \tag 8$
3)若 $r_{31}\ne\pm1$ ，则 $b\ne\pm \pi/2$ ， $cos(b)\ne0$ 。
$a, c$ 的一个解：
$\begin{cases} a= atan2(r_{32},r_{33})\\ c= atan2(r_{21},r_{11})\\ \tag 9 \end{cases}$
$a, c$ 的另一个解：
$\begin{cases} a=atan2(-r_{32},-r_{33})\\ c= atan2(-r_{21},-r_{11}) \\ \tag {10} \end{cases}$
若 $∣ c o s (c) ∣ > ∣ s i n (c) ∣$ ,
$b=atan2(-r_{31},r_{11}/cos(c)) \tag {11}$
否则，
$b=atan2(-r_{31},r_{21}/sin(c)) \tag {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\pm1$ 时，rpy角有两组解。类似地，当旋转矩阵用轴角表示时，有两组解：绕轴 $k$ 旋转 $\theta$ 角度与绕轴 $- k$ 旋转 $-\theta$ 角度；当旋转矩阵用单位四元数表示时，有两组解：单位四元数 $q$ 与单位四元数的相反数 $- q$ 。因而，在姿态的单位四元数SLERP插补中，有必要选取较短的“姿态路径”,使得机器人姿态动作看起来柔顺一些。
(3)当 $r_{31}=1或-1$ 时，一个旋转矩阵对应无穷多组rpy角。当 $r_{31}\ne\pm1$ 时，一个旋转矩阵对应两组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时，判断奇异点的容差值尽可能小。