【机器人学与计算机视觉基础】（一）位置与姿态描述 1 位姿的抽象符号表示

（二） 二维空间位姿描述

讨论2维空间通常要用到笛卡儿坐标系，笛卡儿坐标系是以$x$轴和$y$轴为正交轴的坐标系，通常绘制成$x$轴水平、$y$轴竖直，两轴的交点称为原点。在笛卡尔坐标系中，平行于坐标轴的单位向量$\hat{x}$和$\hat{y}$表示。一个点用其在$x$轴和$y$轴上的坐标$(x,y)$表示,或者写成向量:
$$\mathbf{p}=x\hat{\mathbf{x}}+y\hat{\mathbf{y}}$$

在上图中的一个坐标系 { B } \{B\} {B}，我们希望用参照系 { A } \{A\} {A}来描述它。可以清楚地看到, { B } \{B\} {B}的原点可以被向量$t=(x，y)$所取代，其姿态可以由$\theta$表示。

因此,位姿的一个具体表示就是三维向量${}^A{\xi }_B\sim (x,y,\theta )$，我们使用符号～表示这两种表示是等价的。遗憾的是，这种表示方法不方便位姿合成，因为$\left(x_1,y_1,{\theta }_1\right)\oplus \left(x_2,y_2,{\theta }_2\right)$两边的位姿都是复杂的三角函数。所以，我们将使用一种不同的方法来表示旋转。

该方法是考虑一个任意点$Р$相对于每个坐标系的向量，并确定${}^{A}p$和${}^{B}p$之间的关系。

再次回到上图，我们将问题分成两部分:旋转,然后平移。

2.1 旋转

先只考虑旋转的情况，我们创建一个新坐标系 { V } \{V\} {V}，其坐标轴平行于坐标系 { A } \{A\} {A}的轴,但其原点与坐标系 { B } \{B\} {B}的原点重合，如下图所示。点$P$可以分别看作在 { V } \{V\} {V}或 { B } \{B\} {B}坐标系中的点。

我们可以将点$P$用 { V } \{V\} {V}中定义坐标轴的单位向量表示为
$$\begin{array}{rl}{}^{v}p& ={}^{V}x{\hat{x}}_V+{}^{V}y{\hat{y}}_V\\ & =\left(\begin{array}{ll}{\hat{x}}_V& {\hat{y}}_V\end{array}\right)\left(\begin{array}{l}{}^{V}x\\ {}^{V}y\end{array}\right)\end{array}$$

上式被写作一个行向量和一个列向量的点积。

坐标系 { B } \{B\} {B}|可以用它的两个正交轴表示，这里用两个单位向量代表:
$$\begin{array}{l}{\hat{x}}_B=\cos \theta {\hat{x}}_V+\sin \theta {\hat{y}}_V\\ {\hat{y}}_B=-\sin \theta {\hat{x}}_V+\cos \theta {\hat{y}}_V\end{array}$$

分解为矩阵形式：
$$\left(\begin{array}{ll}{\hat{x}}_B& {\hat{y}}_B\end{array}\right)=\begin{array}{ll}({\hat{x}}_V& {\hat{y}}_V)\end{array}\left(\begin{array}{cc}\cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{array}\right)$$

$P$在坐标系 { B } \{B\} {B}中表示为：
$${}^{B}p={}^{B}x{\hat{x}}_B+{}^{B}y{\hat{y}}_B=\left(\begin{array}{ll}{\hat{x}}_B& {\hat{y}}_B\end{array}\right)\left(\begin{array}{l}{}^{B}x\\ {}^{B}y\end{array}\right)$$

根据上面两个式子，可以得到：
$${}^{B}p=(\begin{array}{ll}{\hat{x}}_V& {\hat{y}}_V)\end{array}\left(\begin{array}{cc}\cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{array}\right)\left(\begin{array}{c}{}^{B}x\\ {}^{B}y\end{array}\right)$$
再对照
$$\begin{array}{rl}{}^{V}p& ={}^{V}x{\hat{x}}_V+{}^{V}y{\hat{y}}_V\\ & =\left(\begin{array}{ll}{\hat{x}}_V& {\hat{y}}_V\end{array}\right)\left(\begin{array}{l}{}^{V}x\\ {}^{V}y\end{array}\right)\end{array}$$
可以发现上面两个式子中同一个点$p$都由单位向量$({\hat{x}}_V,{\hat{y}}_V)$表示，故其右边的系数也应该相同。即：
$$\left(\begin{array}{c}{}^{V}x\\ {}^{V}y\end{array}\right)=\left(\begin{array}{cc}\cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{array}\right)\left(\begin{array}{c}{}^{B}x\\ {}^{B}y\end{array}\right)$$

上式描述了点如何通过坐标系旋转从坐标系 { B } \{B\} {B}变换到坐标系 { V } \{V\} {V}。这种类型的矩阵被称为旋转矩阵,记作${}^V{R}_B$
$$\left(\begin{array}{c}{}^{V}x\\ {}^{V}y\end{array}\right){=}^V{R}_B\left(\begin{array}{c}{}^{B}x\\ {}^{B}y\end{array}\right)$$

旋转矩阵有一些特殊的属性：

• 正规化（标准正交）：每列都是单位向量且相互正交
• 行列式为+1：一个向量被旋转矩阵变换后的长度是不变的
• 逆矩阵和转置矩阵相同$R^{-1}=R^T$，因此有：
  $$\left(\begin{array}{c}{}^{B}x\\ {}^{B}y\end{array}\right)={\left({}^V{R}_B\right)}^{-1}\left(\begin{array}{c}{}^{V}x\\ {}^{V}y\end{array}\right)={\left({}^V{R}_B\right)}^T\left(\begin{array}{l}{}^{V}x\\ {}^{V}y\end{array}\right)={}^B{R}_V\left(\begin{array}{c}{}^{V}x\\ {}^{V}y\end{array}\right)$$
  我们注意到，该矩阵的求逆就是将矩阵的上、下标交换位置，并可以得出恒等式
  $$\mathbf{R}(-\mathbf{\theta })=\mathbf{R}(\mathbf{\theta }{)}^{\mathrm{T}}$$

这里我们可以观察到：我们描述一个旋转的时候，不是用代表旋转角度的一个标量，而是用了一个有4个元素的2×2矩阵。但这4个元素并不是独立的，矩阵的每一列都是一个单位的大小，这提供了两个约束，列与列之间还都是正交的，这提供了另一种约束。4个元素加上3个约束，这样还是只剩下1个真正独立的值。

旋转矩阵是一个非最小化表示的典型例子,虽然这种表示有一些缺点，诸如需要增加内存等，但它具备的优势更加突出，如可复合性。

2.2 平移

接下来讨论坐标系原点的平移

由于坐标系 { V } \{V\} {V}与坐标系 { A } \{A\} {A}是平行的，所以平移量可以直接相加。

$$\begin{array}{rl}\left(\begin{array}{l}{}^{A}x\\ {}^{A}y\end{array}\right)& =\left(\begin{array}{l}{}^{V}x\\ {}^{V}y\end{array}\right)+\left(\begin{array}{l}x\\ y\end{array}\right)\\ & =\left(\begin{array}{cc}\cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{array}\right)\left(\begin{array}{l}{}^{B}x\\ {}^{B}y\end{array}\right)+\left(\begin{array}{l}x\\ y\end{array}\right)\\ & =\left(\begin{array}{ccc}\cos \theta & -\sin \theta & x\\ \sin \theta & \cos \theta & y\end{array}\right)\left(\begin{array}{c}{}^{B}x\\ {}^{B}y\\ 1\end{array}\right)\end{array}$$

或简写成：
$$\left(\begin{array}{c}{}^{A}x\\ {}^{A}y\\ 1\end{array}\right)=\left(\begin{array}{ll}{}^A{R}_B& t\\ 0_{1\times 2}& 1\end{array}\right)\left(\begin{array}{l}{}^{B}x\\ {}^{B}y\\ 1\end{array}\right)$$

其中, $t=(x,y)$代表坐标系的平移变换，而坐标系旋转变换用${}^A{R}_B$表示。因为 { A } \{A\} {A}和 { V } \{V\} {V}的轴是平行的,所以${}^A{R}_B{=}^V{R}_B$。将$P$点的坐标向量用齐次形式表达为
$$\begin{array}{rl}{}^A\tilde{p}& =\left(\begin{array}{ll}{}^V{\mathbf{R}}_B& \mathbf{t}\\ 0_{1\times 2}& 1\end{array}\right){}^B\tilde{\mathbf{p}}\\ & ={}^A{\mathbf{T}}_B{}^B\tilde{\mathbf{p}}\end{array}$$

${}^A{T}_B$称为齐次转换矩阵。这个矩阵有一个非常特殊的结构，并且属于特殊的二维欧几里得群，即$T\in SE(2)\subset {\mathbb{R}}^{3\times 3}$

---

背景材料:一个向量$(x,y)$可以写成齐次形式$\tilde{p}\in \mathbb{P},\tilde{p}=\left(x_1,x_2,x_3\right)$，其中$x=x_1/x_3,y=x_2/x_3,x_3\ne 0$。这里齐次向量的维度增加了一个,在平面上的点就用三维向量表示。为了将点转换为齐次形式,我们通常会附加一个1,变为$\bar{p}=(x,y,1)$。字母上的波浪线表示该向量是齐次形式的。

齐次向量具有一个重要的属性，即$\tilde{p}$等价于入$\lambda \tilde{p},\forall \lambda \ne 0$，记作$\tilde{p}\simeq \lambda \tilde{p}$。这意味着$\tilde{p}$在平面中代表相同的点，而与比例系数无关。

---

${}^A{T}_B$代表了相对位姿：
$$\xi (x,y,\theta )\sim \left(\begin{array}{ccc}\cos \theta & -\sin \theta & x\\ \sin \theta & \cos \theta & y\\ 0& 0& 1\end{array}\right)$$

相对位姿$\xi$的一种具体表示是$\xi \sim T\in SE(2)$。

对应于【机器人学与计算机视觉基础】（一）位置与姿态描述 1 位姿的抽象符号表示 中提到的代数运算规则，有：
$${\mathbf{T}}_1\oplus {\mathbf{T}}_2\mapsto {\mathbf{T}}_1{\mathbf{T}}_2$$
$$T_1{T}_2=\left(\begin{array}{cc}{R}_1& t_1\\ 0_{1\times 2}& 1\end{array}\right)\left(\begin{array}{cc}{R}_2& t_2\\ 0_{1\times 2}& 1\end{array}\right)=\left(\begin{array}{cc}{R}_1{R}_2& t_1+R_1{t}_2\\ 0_{1\times 2}& 1\end{array}\right)$$
这是标准的矩阵乘法。

$$\xi \oplus 0=\xi \mapsto TI=T$$

其中$I$为单位矩阵。

$$\xi \ominus \xi =0\mapsto T{T}^{-1}=I$$

这意味着
$$\ominus \mathbf{T}\mapsto {\mathbf{T}}^{-1}$$
且：
$${\mathbf{T}}^{-1}={\left(\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0_{1\times 2}& 1\end{array}\right)}^{-1}=\left(\begin{array}{cc}{\mathbf{R}}^T& -{\mathbf{R}}^T\mathbf{t}\\ 0_{1\times 2}& 1\end{array}\right)$$

对于点$\tilde{p}\in {\mathbb{P}}^2$，有$\mathbf{T}\cdot \tilde{\mathbf{p}}\mapsto T\tilde{\mathbf{p}}$，这是一个标准的矩阵-向量积。

下面我们将使用MATLAB机器人工具箱（关注微信公众号二进制人工智能回复robot获取）展示一些具体数值化的例子。用函数se2创建一个齐次变换

T1=se2(1,2,30*pi/180)
T1 =

    0.8660   -0.5000    1.0000
    0.5000    0.8660    2.0000
         0         0    1.0000

代表（1，2）的平移和30度的旋转。

相对世界坐标系，我们可以将变换用绘图函数绘制如下：

trplot2(T1,'frame','1','color','b');
axis([0,5,0,5]);

绘图函数中的参数规定该坐标系标签为 { 1 } \{1\} {1}，且绘制成蓝色。

我们再绘制另一个平移（2，1），零旋转的位姿。在图中用红色绘制出来

T2=se2(2,1,0);
hold on
trplot2(T2,'frame','2','color','r');

现在，我们将两个相对位姿复合

T3=T1*T2;
trplot2(T3,'frame','3','color','g');

我们可以看到复合中，坐标系{2}的平移（2，1）是相对于坐标系{1}的。

要注意的是，最终坐标系{3}相对于世界坐标系的平移量并不是（3，3），因为它是相对于旋转后的坐标系{1}再平移的。

复合运算的不可逆性可用以下例子证明：

T4=T2*T1;
trplot2(T4,'frame','4','color','c');

可以看出坐标系{4}和坐标系{3}是不同的。

现在我们相对于世界坐标系定义一个点(3,2)，并把它标到图中

P=[3;2];
plot_point(P,'*');

确定该点相对于坐标系{1}的坐标：
由
$${}^0\mathbf{p}={}^0{\xi }_1\cdot {}^1\mathbf{p}$$
得到：
$${}^1\mathbf{p}={}^1{\xi }_0\cdot {}^0\mathbf{p}={\left({}^0{\xi }_1\right)}^{-1}\cdot {}^0\mathbf{p}$$

P1=inv(T1)*[P;1] % [P,1]：附加一个1将欧几里得点转换为齐次形式
P1 =

    1.7321
   -1.0000
    1.0000

对齐次形式进行反变换，变为欧几里得坐标点

h2e(inv(T1)*e2h(P))
ans =

    1.7321
   -1.0000

这里又变回了欧几里得坐标点结果。辅助函数e2h将欧几里得坐标点转换为齐次形式，而h2e则进行逆转换。更简洁的表达方式可写成

上面更加简洁的表达可写成：

homtrans(inv(T1),P)
ans =

    1.7321
   -1.0000

THE END.
感谢阅读。