机器人学中旋转矩阵R与RPY角的转换关系，附matlab代码

最近在ROS中调试机械臂抓取实验，经常用到坐标系之间的转换关系，特别是在配置相机与机械臂位置时，无论是在urdf加载障碍物或表示连杆之间相对位姿，再如launch文件中用到各坐标系转换时，总是用到x y z r p y，xyz没什么好说的，就是坐标系原点的位置关系，但是rpy算起来比较麻烦，加上个人空间感太差，一致弄不清楚到底是绕哪个轴旋转多少角度。但是用旋转矩阵R表示两个坐标系之间的关系则很简单，例如坐标系B对A的旋转矩阵，就是B的各个坐标轴在坐标系A下是如何表示的，详细一点可以用Peter Corke的那本经典的机器人学、机器视觉与控制书中讲到的，ps：作者是matlab机器人工具箱RTB的开发者：
在正交旋转矩阵中，从左至右的列向量可以告诉我们旋转后新坐标系各个轴相对于当前坐标系的方向。例如，如果

则新坐标系的x轴仍在以前x轴方向（1，0，0），但其y轴却在以前的z轴方向（0，0，1），而新的z轴在以前y轴的反方向（0，-1，0）。这里，之所以x轴没变，是因为这个旋转是围绕x轴发生的。行向量则相反，它们表示了当前坐标系的各个轴在新坐标系中的方向。
进入正题：
已知坐标系B相对于坐标系A的旋转矩阵R,R就是B的各个坐标轴在A中的表示，通过调用matlab中的函数
tr2rpy( R )即可求得rpy三个角度，分别为绕坐标系A的xyz轴的旋转角度，因此在Craig的机器人学导论书中，rpy也是xyz固定角，因为旋转是绕固定坐标系的固定轴旋转的。