平面坐标系变换

1. 点（基本元素）
2. 坐标变换运算
3. 变换的齐次矩阵
4. 运算的性质

机器人学中常涉及不同坐标系间的变换，在ROS中可以使用TF，但某些情况下也有手写变换的必要。

点（基本元素）

平面直角坐标系中，一个刚体的坐标和姿态（位姿）表现为向量$(x,y,\alpha)$，且其它刚体的位姿相对它有唯一表示。因此该向量（通常写成列向量的形式）便是平面直角坐标系坐标变换研究的基本元素（点）。

坐标系O下的位姿P表示为 $\begin{pmatrix} x\ y\ \alpha \end{pmatrix} ^O _P$ 或齐次形式 $\begin{pmatrix} x\ y\ \alpha\ 1 \end{pmatrix} ^O _P$, 该位姿也可作为新坐标系的原点。

坐标变换运算

当以$(x,y,\alpha)^O_P$为新坐标系原点时，即产生坐标系P与坐标系O间的对应关系。该变换是平面欧几里德变换，因此是一一对应的，且仅取决于P与O的位姿关系$P^O$。

$F(P \to O) = \Phi(P^O)$, 为平面上的任意点 $p$ 在 $P$ 和在 $O$ 坐标系中的坐标对应关系 $F(p^P) \to p^O$

令$Rot(\alpha) = \bigl(\begin{smallmatrix}\cos\alpha&-\sin\alpha\\sin\alpha&\cos\alpha\end{smallmatrix}\bigr)$ 为逆时针（正向）旋转$\alpha$的旋转阵，显然$p^P \to p^O$为$p$旋转$\alpha^O_P$，再平移$(x,y)$所得，有：

$$\begin{pmatrix}x\\y\\\alpha\end{pmatrix}^O = F(\begin{pmatrix}x\\y\\\alpha\end{pmatrix}^P) = \begin{bmatrix} Rot(\alpha^O_P)\begin{pmatrix}x\\y\\\end{pmatrix}^P + \begin{pmatrix}x\\y\\\end{pmatrix}^O_P \\ \alpha^P + \alpha^O_P \end{bmatrix}$$

变换的齐次矩阵

使用齐次坐标，可以写成变换矩阵 $T$ 与齐次坐标 $p$ 相乘的形式：

$$F(p^P) = \begin{bmatrix}\cos\alpha&-\sin\alpha&0&x \\ \sin\alpha&\cos\alpha&0&y \\ 0&0&1&\alpha \\ 0&0&0&1\end{bmatrix}^O_P \begin{pmatrix}x\\y\\\alpha\\1\end{pmatrix}^P = T^O_P p^P$$

可见$T^O_P = T(P^O)$也是$P^O$的函数。

令$Rot_z(\alpha) = \begin{bmatrix}\cos\alpha&-\sin\alpha&0\\\sin\alpha&\cos\alpha&0\\ 0&0&1\end{bmatrix}$即空间中绕z轴的旋转阵，可将$T$表示成分块矩阵：

$$F(p^P) = T^O_P p^P = \begin{bmatrix}Rot_z(\alpha^O_P)&P^O\\0&1\end{bmatrix} \begin{pmatrix}p^P\\1\end{pmatrix}$$

运算的性质

$F$ 与 $T$ 的运算律间有明显的对应关系：

• 传递性

$$F(O \to N) = F(O \to M) F(M \to N) \iff T^O_N = T^O_M T^M_N$$

• 逆变换

$$F(P \to O) = F^{-1}(O \to P) \iff T^P_O = (T^O_P)^{-1} = \begin{bmatrix}Rot_z(-\alpha^O_P)& -Rot_z(-\alpha^O_P)P^O\\0&1\end{bmatrix}$$

• 等效性

$$F^O_M F^M_N = F^O_N = \Phi(P^O_N) = \Phi(F^O_M(P^M_N)) \iff T^O_M T^M_N = T(T^O_M P^M_N)$$

因为

$$左边 = \begin{bmatrix}Rot_z(\alpha^O_M)&P^O_M\\0&1\end{bmatrix} \begin{bmatrix}Rot_z(\alpha^M_N)&P^M_N\\0&1\end{bmatrix} = \begin{bmatrix}Rot_z(\alpha^O_M+\alpha^M_N)&Rot_z(\alpha^O_M)P^M_N+P^O_M\\0&1\end{bmatrix}$$

而

$$右边 = T(\begin{bmatrix}Rot_z(\alpha^O_M)&P^O_M\\0&1\end{bmatrix}\begin{pmatrix}P^M_N\\1\end{pmatrix}) = T(Rot_z(\alpha^O_M)P^M_N+P^O_M)$$

所以 左边=右边