机器人基础-齐次变换

Homogeneous Transformation（齐次变换） 是计算机视觉、机器人学和图形学中常用的数学工具，用于描述物体在空间中的平移、旋转、缩放等操作。它将这些操作统一表示为一个矩阵乘法操作，使得多个变换可以通过矩阵相乘来组合。

齐次变换概念

齐次变换的核心思想是将三维（或二维）空间中的点表示为四维（或三维）向量，然后使用 4x4（或 3x3）矩阵来表示变换。这种表示方式的好处在于，可以将平移、旋转、缩放等线性变换统一起来，并且可以轻松地进行组合。

齐次坐标

在二维空间中，一个点通常表示为 $(x,y)$，将其转换为齐次坐标表示时，会增加一个附加分量，通常为 1：
$$p=\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]$$
在三维空间中，一个点的齐次坐标可以表示为：
$$p=\left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]$$

齐次变换矩阵

齐次变换矩阵包含了旋转矩阵和平移向量，能够同时表示平移的旋转操作：
$$\mathbf{T}=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0^T& 1\end{array}\right]$$

• $\mathbf{R}$: 3x3 旋转矩阵，表示在三维空间中的旋转。
• $\mathbf{t}$: 3x1 平移向量，表示在三维空间中的平移。
• $0$: 3x1 的零向量。
• $1$: 标量，用于保持矩阵的尺寸一致。

在二维空间中，齐次变换矩阵通常是 3x3 矩阵：

$$\mathbf{T}=\left[\begin{array}{ccc}\cos \theta & -\sin \theta & t_x\\ \sin \theta & \cos \theta & t_y\\ 0& 0& 1\end{array}\right]$$

在三维空间中，齐次变换矩阵通常是一个 4x4 矩阵。它同时包含旋转矩阵和平移向量，用于表示三维空间中的平移和旋转操作：

$$\mathbf{T}=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0^T& 1\end{array}\right]$$

• $\mathbf{R}$: 3x3 旋转矩阵，用于表示三维空间中的旋转。
• $\mathbf{t}$: 3x1 平移向量，用于表示三维空间中的平移。
• $0$: 3x1 的零向量。
• $1$: 标量，用于保持矩阵的尺寸一致。

其中，

1. 绕 (x) 轴旋转：

   $$R_x(\theta )=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \theta & -\sin \theta \\ 0& \sin \theta & \cos \theta \end{array}\right]$$

2. 绕 (y) 轴旋转：

   $$R_y(\theta )=\left[\begin{array}{ccc}\cos \theta & 0& \sin \theta \\ 0& 1& 0\\ -\sin \theta & 0& \cos \theta \end{array}\right]$$

3. 绕 (z) 轴旋转：

   $$R_z(\theta )=\left[\begin{array}{ccc}\cos \theta & -\sin \theta & 0\\ \sin \theta & \cos \theta & 0\\ 0& 0& 1\end{array}\right]$$
   如果物体在三维空间中同时绕 𝑥轴、𝑦轴和 𝑧轴旋转，综合旋转矩阵可以表示为这些基本旋转矩阵的乘积。注意，矩阵乘法的顺序很重要，通常先绕 𝑧轴，再绕 𝑦轴，最后绕 𝑥轴进行旋转：
   $$R(\alpha ,\beta ,\gamma )=R_z(\gamma )R_y(\beta )R_x(\alpha )$$
   $$R(\alpha ,\beta ,\gamma )=\left[\begin{array}{ccc}\cos \beta \cos \gamma & \cos \gamma \sin \alpha \sin \beta -\cos \alpha \sin \gamma & \cos \alpha \cos \gamma \sin \beta +\sin \alpha \sin \gamma \\ \cos \beta \sin \gamma & \cos \alpha \cos \gamma +\sin \alpha \sin \beta \sin \gamma & -\cos \gamma \sin \alpha +\cos \alpha \sin \beta \sin \gamma \\ -\sin \beta & \cos \beta \sin \alpha & \cos \alpha \cos \beta \end{array}\right]$$

齐次变换矩阵可以将三维空间中的一个点 $\mathbf{p}=[x,y,z{]}^T$转换到新的坐标系中：

$${\mathbf{p}}^{\prime }=\mathbf{T}\cdot \left[\begin{array}{c}x\\ y\\ z\\ 1\end{array}\right]$$

其中，${\mathbf{p}}^{\prime }=[x^{\prime },y^{\prime },z^{\prime }{]}^T$表示变换后的新坐标。

齐次变换的应用

假设我们有一个机器人，它在地球坐标系中的位置为 $(x_0,y_0,z_0)=(5,10,0)$，并且它绕 $z $ 轴旋转了 90 度（ $\theta =90^{\circ }$）。

1. 齐次变换矩阵

旋转矩阵 $R_z(\theta )$ 和平移向量 $t$ 如下：

$$R_z\left(\frac{\pi }{2}\right)=\left[\begin{array}{ccc}0& -1& 0\\ 1& 0& 0\\ 0& 0& 1\end{array}\right],\mathbf{t}=\left[\begin{array}{c}5\\ 10\\ 0\end{array}\right]$$

齐次变换矩阵 $T$ 是：

$$\mathbf{T}=\left[\begin{array}{cccc}0& -1& 0& 5\\ 1& 0& 0& 10\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$

2. 转换点的坐标

假设机器人坐标系中的点为 $(x_r,y_r,z_r)=(1,2,0)$，表示为齐次坐标：

$${\mathbf{p}}_{robot}=\left[\begin{array}{c}1\\ 2\\ 0\\ 1\end{array}\right]$$

将该点转换到地球坐标系中：

$${\mathbf{p}}_{world}=\mathbf{T}\cdot {\mathbf{p}}_{robot}=\left[\begin{array}{c}3\\ 11\\ 0\\ 1\end{array}\right]$$

因此，机器人坐标系中的点 $(1,2,0)$在地球坐标系中的位置为 $(3,11,0)$。