位置、方向和坐标变换

为了表示一个刚体相对于另一个刚体的相对位姿（位置和方向），需要将坐标系附加到每个刚体上，然后规定了这两个坐标系之间的几何关系。

位置

一个帧的原点相对于另一个帧的位置可以 用 translation vector （3x1），即平移向量来表述：

\[\begin{split}\boldsymbol{t} = \begin{bmatrix} x \\ y \\ z \end{bmatrix}\end{split}\]

方向

一个坐标系相对于另一个坐标系的方向可以 用 rotation matrix （3x3），即旋转矩阵来表述：

\[\begin{split}\boldsymbol{R} = \begin{bmatrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \\ \end{bmatrix}\end{split}\]

位姿

一 个 homogeneous transformation matrix （4x4），即齐次变换矩阵，可同时表示一个坐标系相对于另一个坐标系的位置和方向：

\[\begin{split}\boldsymbol{H} = \begin{bmatrix} \boldsymbol{R} & \boldsymbol{t}\\ 0 & 1\\ \end{bmatrix} = \begin{bmatrix} r_{11} & r_{12} & r_{13} & x\\ r_{21} & r_{22} & r_{23} & y\\ r_{31} & r_{32} & r_{33} & z\\ 0 & 0 & 0 & 1\\ \end{bmatrix}\end{split}\]

Roll-Pitch-Yaw

一个刚体最多具有三个旋转自由度。换句话说，只需要三个独立的量足以描述任意刚体方向。一种表示方法 为 roll-pitch-yaw representation，由Roll（\(\phi\)），Pitch（\(\theta\)）和Yaw（\(\psi\)）角度定义：

\[\boldsymbol{R}(\phi, \theta, \psi)\]

Roll-pitch-yaw表示方法的优点是它只需要三个值。与其他表示方法相比，这些值在几何上也很容易理解。缺点是：

  • 这些值不是连续的。

  • 最终方向取决于：

    • 旋转的顺序。

    • 围绕移动轴（内部旋转）还是固定轴（外部旋转）旋转。

机器人厂家常使用Roll-Pitch-Yaw表示方法。但是，并非所有机器人供应商都采用相同的惯例。了解旋转顺序以及它们是内部旋转还是外部旋转，这些是将Roll-Pitch-Yaw角度转换为其他表示形式的必要条件。

轴-角/旋转矢量

Axis-angle 表示方法描述了以下内容：

  • 一个代表旋转方向轴的单位矢量 (\(\boldsymbol{u}\))。

  • 一个描述绕轴旋转的角度（\(\theta\)）

一共有四个参数。为了最大程度地减少参数数量，通常将单位向量的每个元素乘以角度，其结果就是旋转向量 (\(\boldsymbol{r}\))：

\[\boldsymbol{r} = \begin{bmatrix} r_x & r_y & r_z \end{bmatrix} = \begin{bmatrix} u_x \theta & u_y \theta & u_z \theta \end{bmatrix}\]

轴-角表示方法相对于roll-pitch-yaw表示方法的优点在于它没有连续性和旋转顺序的问题。但是很难在物理方向和旋转矢量的数值之间进行匹配。另一个缺点是不能将旋转直接应用于3D点，而需要转换为其它表示方法。

单位四元数

Unit quaternions 是一种简单但具有鲁棒性的方法，用四个参数对轴-角表示方法进行编码。

\[\boldsymbol{q} = \begin{bmatrix} q_{w} & q_{x} & q_{y} & q_{z} \end{bmatrix}\]

单位四元数被认为是表示两个坐标系之间方向的最佳方法，因为它们比旋转矩阵更紧凑、数值更稳定且更高效。与roll-pitch-yaw表示方法相比，单位四元数不会受到万向节死锁的影响（不可能唯一地表示方向）。此外，与轴-角表示不同，可以将单位四元数直接应用于3D点。

备注

查看 常见的姿态表示方法之间的转换。

坐标转换

Any coordinate transformation of a rigid body in 3D can be described with a rotation and a translation. For example, it is possible to transform a point (or a point cloud) from one coordinate frame (\(b\)) to another coordinate frame (\(a\)). This can be performed with a rotation matrix and a translation vector:

\[p^{a} = \boldsymbol{R}^{a}_{b} p^{b} + \boldsymbol{t}^{a}\]

也可以使用描述两个坐标系之间的位姿的 齐次变换矩阵 来实现：

\[\widetilde{p}^{a} = \boldsymbol{H}^{a}_{b} \widetilde{p}^{b}\]

where \(\widetilde{p}^{a}\) and \(\widetilde{p}^{b}\) are homogeneous representations of the vectors \(p^{a}\) and \(p^{b}\), respectively.

The homogeneous transformation matrix \(\boldsymbol{H}^{a}_{b}\) in the above equation represents:

  • the operation that transforms a point \(p^{b}\) in frame \(b\) to a point \(p^{a}\) in frame \(a\) (coordinate transformation matrix)

  • the operation that expresses a point \(p^{b}\) defined relative to frame \(b\) as it would be defined relative to frame \(a\) (coordinate transformation matrix)

  • the pose (position and orientation) of frame \(b\) with respect to frame \(a\)

  • the transform (position and orientation) from frame \(a\) to frame \(b\) expressed as a homogenous transformation matrix

In code samples, we use the following notation for this transformation matrix: a_to_b_transform or b_pose_in_a_frame.