常见的姿态表示方法之间的转换
本文介绍了常见的机器人姿态表示方法之间的转换方程。有关位姿表示的更多信息,请查看 位置、方向和坐标变换 。
Roll-Pitch-Yaw到旋转矩阵
Roll-pitch-yaw是表示方向的常用术语。每一个代表了围绕单个轴的旋转角度,它们组合起来描述了完整的旋转角度。但是,它们所表示的确切意义并不清楚。有以下困惑:
这些角度是绕哪个轴的旋转角度?
这些轴是固定的还是移动的?
旋转定义的顺序是什么(有多种可能性)?
旋转顺序通常表示 为 x-y-z
或者 z-y'-x''
。这里的 x
, y
和 z
表示它围绕其旋转的轴 。 '
用于指示轴是否是固定的。绕固定轴旋转称为 extrinsic rotation
。围绕移动轴的旋转称为 intrinsic rotation
。
下面是两个不同的例子。
对于 x-y-z
或者 z-y'-x''
, roll \(\phi\) ,pitch \(\theta\) 和yaw \(\psi\) 角度可以转换为下面的旋转矩阵 \(R\) :
对 于 z-y-x
或者 x-y'-z''
, rol l \(\phi\) , pitch \(\theta\) , 和yaw \(\psi\) 角度可以转换为下面的旋转矩阵 \(R\) :
备注
即使两个示例中的角度是相同的,它们也不代表相同的最终旋转矩阵。
即使最终旋转矩阵是相同的,两个示例之间的角度也不相同。
这里引入了一个定义:roll分配给绕运动轴的第一个旋转,pitch是第二个,yaw是第三个。
旋转矢量到轴-角
一个旋转向量 \(\boldsymbol{r}\) 可以转换成轴 \(\boldsymbol{u}\) 和角度 \(\theta\) ,如下所示:
轴-角到四元数
轴 \(\boldsymbol{u}\) 和角度 \(\theta\) 可以转换为单位四元数 \(\boldsymbol{q}\) ,如下所示:
四元数到旋转矩阵
单位四元数 \(\boldsymbol{q}\) 可以转换为旋转矩阵 \(\boldsymbol{R}\) ,如下所示:
假设四元数已归一化 \((q_w + q_x + q_y + q_z = 1)\) 。如果不是,则应在使用以下公式进行转换之前将其归一化:
const Eigen::Matrix3f rotationMatrixFromQuaternion = quaternion.toRotationMatrix();
std::cout << "Rotation Matrix from Quaternion:\n"
<< rotationMatrixFromQuaternion.format(matrixFormatRules) << std::endl;
旋转矩阵到四元数
旋转矩阵 \(\boldsymbol{R}\) 可以转换为单位四元数 \(\boldsymbol{q}\) ,如下所示:
四元数到轴-角
单位四元 数 \(\boldsymbol{q}\) 可以转换成轴 \(\boldsymbol{u}\) 和角度 \(\theta\) ,如下所示:
这对于从旋转矩阵转换为轴-角很有用。请参阅下面的代码示例实现。
const Eigen::AngleAxisf axisAngle(rotationMatrix);
std::cout << "AxisAngle:\n"
<< axisAngle.axis().format(vectorFormatRules) << ", " << axisAngle.angle() << std::endl;
轴-角到旋转矢量
轴 \(\boldsymbol{u}\) 和角度 \(\theta\) 可以转换为旋转向量 \(\boldsymbol{r}\) ,如下所示:
这对于将旋转矩阵转换为旋转向量很有用。请参阅下面的代码示例实现。
const Eigen::Vector3f rotationVector = rotationMatrixToRotationVector(rotationMatrix);
std::cout << "Rotation Vector:\n" << rotationVector.format(vectorFormatRules) << std::endl;
旋转矩阵到Roll-Pitch-Yaw
从旋转矩阵确定滚动、俯仰、偏航角并不简单。可以有多个,有时甚至是无数个解。这就需要一种算法,该算法可以根据某些标准从多个解中选择出其中一个解。