机器人学——1.1-二维空间位姿描述

二维空间位姿描述

二维世界或平面，是我们在高中学习欧几里得几何时就熟悉的。笛卡儿坐标系，或以 $x$ 轴和 $y$ 轴为正交轴的坐标系，通常绘制成 $x$ 轴水平、$y$ 轴竖直，两轴的交点称为原点。平行于坐标轴的单位向量用 $hat{x}$ 和 $hat{y}$ 表示。一个点用其在 $x$ 轴和 $y$ 轴上的坐标 $left(x,y ight)$ 表示，或者写为有界向量：
$ag{1} p= ext{x}hat{x}+ ext{y}hat{x}$
在下图中的一个坐标系 ${B}$ ，我们希望用参照系 ${A}$ 来描述它。可以清楚地看到，${B}$ 的原点已被向量$t=left(x, y ight)$所取代，然后逆时针旋转一个角度 $heta$ 。因此，位姿的一个具体表示就是三维向量 $^Axi_B sim left(x, y, heta ight)$ ，我们使用符号 $sim$ 表示这两种表示是等价的。

遗憾的是，这种表示方法不方便复合，因为：
$left(x_1, y_1, heta_1 ight)oplusleft(x_2, y_2, heta_2 ight)$两边的位姿都是复杂的三角函数。所以，我们将使用一种不同的方法来表示旋转。

该方法是考虑一个任意点 $P$ 相对于每个坐标系的向量，并确定 $^Ap$ 和 $^Bp$ 之间的关系。再次回到上图，我们将问题分成两部分：旋转，然后平移。

先只考虑旋转的情况，我们创建一个新坐标系 ${V}$ ，其坐标轴平行于坐标系 ${A}$ 的轴，但其原点与坐标系 ${B}$ 的原点重合，如下图所示。

根据方程 $left(1 ight)$ ，我们可以将点 $P$ 用 ${V}$ 中定义坐标轴的单位向量表示为
$ag{2} egin{array}{rl} ^Vp= & {}^V ext{x}hat{x}_V+{}^V ext{y}hat{y}_V \[1em] = & left(egin{array}{cc}hat{x}_V&hat{y}_Vend{array} ight)left(egin{array}{c}^V ext{x}\^V ext{y}end{array} ight) end{array}$上式被写作一个行向量和一个列向量的点积。

坐标系 ${B}$ 可以用它的两个正交轴表示，这里用两个单位向量代表：
$egin{array}{rl} hat{x}_B= & cos heta hat{x}_V + sin hetahat{y}_V \ hat{y}_B= & -sin heta hat{x}_V + cos hetahat{y}_V end{array}$上式写作矩阵形式为：
$ag{3} left(egin{array}{cc}hat{x}_B & hat{y}_Bend{array} ight)= left(egin{array}{cc}hat{x}_V & hat{y}_Vend{array} ight)left(egin{array}{cc}cos heta & -sin heta \ sin heta & cos hetaend{array} ight)$用方程 $(1)$ 可以在坐标系 ${B}$ 中将 $P$ 点表示为
$^Bp={}^B ext{x}hat{x}_B+{}^B ext{y}hat{y}_B=left(egin{array}{cc}hat{x}_B&hat{y}_Bend{array} ight)left(egin{array}{c}^B ext{x}\^B ext{y}end{array} ight)$代入方程 $(3)$ 得
$ag{4} ^Bp=left(egin{array}{cc}hat{x}_V & hat{y}_Vend{array} ight)left(egin{array}{cc}cos heta & -sin heta \ sin heta & cos hetaend{array} ight)left(egin{array}{c}^B ext{x}\^B ext{y}end{array} ight)$由于 $^Bp$ 和 $^Vp$ 都是点 $P$ 的表现形式，现在令方程 $(2)$ 和方程 $(4)$ 各自右侧相等，可得
$left(egin{array}{c}^V ext{x}\^V ext{y}end{array} ight) = left(egin{array}{cc}cos heta & -sin heta \ sin heta & cos hetaend{array} ight)left(egin{array}{c}^B ext{x}\^B ext{y}end{array} ight)$上式描述了点如何通过坐标系旋转从坐标系 ${B}$ 变换到坐标系 ${V}$ 。这种类型的矩阵被称为旋转矩阵，记作 $^VR_B$（这个记号和位置与姿态概述中的 $(1)$ 式表述一致）。
$ag{5} left(egin{array}{c}^V ext{x}\^V ext{y}end{array} ight) = {}^VR_Bleft(egin{array}{c}^B ext{x}\^B ext{y}end{array} ight)$
旋转矩阵 $^VR_B$ 具有一些特殊的属性。首先，它是正规化的（也称为标准正交），因为它的每列都是单位向量且相互正交的。实际上矩阵的每列都是简单地将 ${B}$ 定义在 ${V}$ 中的单位向量，因此根据定义它们都是单位长度且正交的。
其次，它的行列式是 $+1$ ，这意味着旋转矩阵 $R$ 属于特殊的二维正交群，或 $R in SO(2) subset mathbb{R}^{2 imes 2}$。而且单位行列式还意味着向量在变换前后的长度是不变的，即 $|^Bp| = |^Vp|$，$forall heta$ 。
正交矩阵有一种非常方便的属性：$R^{-1}=R^T$，即它的逆矩阵和转置矩阵相同。因此，我们可以重新将方程 $(5)$ 整理为
$left(egin{array}{c}^B ext{x}\^B ext{y}end{array} ight) = left({}^VR_B ight)^{-1}left(egin{array}{c}^V ext{x}\^V ext{y}end{array} ight) = left({}^VR_B ight)^{T}left(egin{array}{c}^V ext{x}\^V ext{y}end{array} ight) = {}^BR_Vleft(egin{array}{c}^V ext{x}\^V ext{y}end{array} ight)$我们注意到，该矩阵的求逆就是将矩阵的上、下标交换位置,并可以得出恒等式 $R(- heta) = R( heta)^T$。

这里我们观察到一个有趣的事实：我们描述一个旋转的时候，不是用代表旋转角度的一个标量，而是用了一个有 $4$ 个元素的 $2 imes 2$ 矩阵。但这 $4$ 个元素并不是独立的，矩阵的每一列都是一个单位的大小，这提供了两个约束，列与列之间还都是正交的，这提供了另一种约束。$4$ 个元素加上 $3$ 个约束，这样还是只剩下 $1$ 个真正独立的值。旋转矩阵是一个非最小化表示的典型例子，虽然这种表示有一些缺点，诸如需要增加内存等，但它具备的优势更加突出，如可复合性。

描述位姿的第二部分是平移。由于坐标系 ${V}$ 和 ${A}$ 的轴是平行的，所以可以简单地进行向量相加：
$ag{6} egin{array}{rl} left(egin{array}{c}^A ext{x}\^A ext{y}end{array} ight) & = left(egin{array}{c}^V ext{x}\^V ext{y}end{array} ight) + left(egin{array}{c}x\yend{array} ight) \[2em] & = left(egin{array}{cc}cos heta & -sin heta \ sin heta & cos hetaend{array} ight)left(egin{array}{c}^B ext{x}\^B ext{y}end{array} ight) + left(egin{array}{c}x\yend{array} ight) \[2em] & = left(egin{array}{ccc}cos heta & -sin heta & x \ sin heta & cos heta & yend{array} ight)left(egin{array}{c}^B ext{x}\^B ext{y}\1end{array} ight) end{array}$或简写为
$ag{7} left(egin{array}{c}^A ext{x}\^A ext{y}\1end{array} ight) = left(egin{array}{cc}{}^AR_B & t \ 0_{1 imes 2} & 1end{array} ight)left(egin{array}{c}^B ext{x}\^B ext{y}\1end{array} ight)$
其中，$t=(x, y)$ 代表坐标系的平移变换，而坐标系旋转变换用 $^AR_B$ 表示。因为 ${A}$ 和 ${V}$ 的轴是平行的，所以 $^AR_B ={}^VR_B$。将 $P$ 点的坐标向量用齐次形式表达为
$egin{array}{rl} ^A ilde{p} &= left(egin{array}{cc}{}^AR_B & t \ 0_{1 imes 2} & 1end{array} ight){}^B ilde{p}\[2em] &={}^AT_B{}^B ilde{p} end{array}$$^AT_B$称为齐次转换矩阵。这个矩阵有一个非常特殊的结构，并且属于特殊的二维欧几里得群，即$T in SE(2) subset mathbb{R}^{3 imes 3}$。

一个向量 $(x, y)$ 可以写成齐次形式$ilde{p} in mathbb{P}$，$ilde{p}=(x_1, x_2, x_3)$，其中 $x=x_1/x_3, y=x_2/x_3, x_3 e 0$。这里齐次向量的维度增加了一个，在平面上的点就用三维向量表示。为了将点转换为齐次形式，我们通常会附加一个1，变为 $ilde{p}=(x,y,1)$。字母上的波浪线表示该向量是齐次形式的。
齐次向量具有一个重要的属性，即 $ilde{p}$ 等价于 $lambda ilde{p}$，$forall lambda e 0$，记作 $ilde{p} simeq lambda ilde{p}$。这意味着 $ilde{p}$ 在平面中代表相同的点，而与比例系数无关。在以后讨论计算机视觉时，齐次表示法非常重要。

与位置与姿态概述中的 $(1)$ 式比较，显然$^AT_B$代表了相对位姿：
$xi(x, y, heta) sim left(egin{array}{ccc}cos heta & -sin heta & x \ sin heta & cos heta & y \ 0 & 0 & 1 end{array} ight)$

相对位姿 $xi$ 的一种具体表示是 $xisim Tin SE(2)$，以及 $T_1 oplus T_2 mapsto T_1T_2$，这是标准的矩阵乘法。
$T_1T_2=left(egin{array}{cc}{}R_1 & t_1 \ 0_{1 imes 2} & 1end{array} ight)left(egin{array}{cc}{}R_2 & t_2 \ 0_{1 imes 2} & 1end{array} ight) = left(egin{array}{cc}{}R_1R_2 & t_1+R_1t_2 \ 0_{1 imes 2} & 1end{array} ight)$位置与姿态概述中给出位姿的代数运算规则之一是 $xioplus 0=xi$。我们知道在矩阵中 $TI=T$，其中 $I$ 是单位矩阵，所以对于位姿 $0$ 对应的是 $I$ 单位矩阵。另一条规则是 $xiominus xi=0$。我们知道矩阵 $TT^{-1} =I$，这意味着 $ominus T mapsto T^{-1}$。
$T^{-1}=left(egin{array}{cc}{}R & t \ 0_{1 imes 2} & 1end{array} ight)^{-1}=left(egin{array}{cc}{}R^T & -R^Tt \ 0_{1 imes 2} & 1end{array} ight)$对于点 $ilde{p} in mathbb{P}$，有 $T cdot ilde{p} mapsto T ilde{p}$，这是一个标准的矩阵-向量积。

MATLAB机器人工具箱举例

下面我们将使用MATLAB机器人工具箱展示一些具体数值化的例子，我的RTB版本是 10.x，安装教程可以百度。

首先是旋转矩阵的例子

>> R = rot2(0.2)
R =
    0.9801   -0.1987
    0.1987    0.9801

其中角度的单位是弧度，我们还可以验证旋转矩阵的一些性质：

>> det(R)
ans =
     1
>> det(R*R)
ans =
     1

机器人工具箱也支持符号变量：

>> syms theta
>> R = rot2(theta)
R =
[ cos(theta), -sin(theta)]
[ sin(theta),  cos(theta)]
>> simplify(R*R)
ans =
[ cos(2*theta), -sin(2*theta)]
[ sin(2*theta),  cos(2*theta)]
>> simplify(det(R))
ans =
1

矩阵指数

假设有一个旋转角度为 0.3 弧度的旋转矩阵

>> R = rot2(0.3)
R =
    0.9553   -0.2955
    0.2955    0.9553

我们可以用 MATLAB 内置函数logm计算矩阵的对数

logm和log对矩阵的运算的不同之处在于，后者对矩阵的每个元素做对数运算，而前者用级数的方法计算矩阵的对数，矩阵的对数是不唯一的，logm计算的是矩阵的 principal logarithm

>> S = logm(R)
S =
         0   -0.3000
    0.3000         0

得到一个元素大小为 0.3 的简单的矩阵，发现了没，之前旋转矩阵的角度也是 0.3。这里发生了一些更深入而有趣的事情，这是已经接近李群的边缘了，我们接下来讨论。

我们假设二维空间中有个矩阵长这样
$left[ omega ight] _{ imes}=left( egin{matrix} 0& -omega\ omega& 0\ end{matrix} ight)$
这个样子的矩阵叫反对称矩阵（或斜对称矩阵）。用机器人工具箱创建一个反对称矩阵如下：

>> skew(2)
ans =
     0    -2
     2     0

skew的逆运算为vex

>> vex(skew(2))
ans =
     2

注意，反对称矩阵的主对角线元素都为零，对于之前的矩阵S：

>> vex(S)
ans =
    0.3000

我们得到了旋转角度。
之前提到的函数logm的逆运算为expm

>> expm(S)
ans =
    0.9553   -0.2955
    0.2955    0.9553

这个结果和rot2(0.3)的结果一样。通常我们这样来表示
$R=e^{[ heta]_ imes}in ext{SO}(2)$
其中 $heta$ 是旋转角，记号 $[cdot]_ imes:mathbb{R}mapstomathbb{R}^{2 imes2}$，表示从一个标量映射到反对称矩阵。

齐次变换矩阵

我们传建一个齐次变换矩阵：平移 $(1,2)$，旋转 $30°$：

>> T1 = transl2(1, 2) * trot2(30, 'deg')
T1 =
    0.8660   -0.5000    1.0000
    0.5000    0.8660    2.0000
         0         0    1.0000

函数transl2表示平移而不旋转，函数trot2表示旋转而不平移。

机器人工具箱中很多函数有对应的齐次变换矩阵形式和正交旋转矩阵形式，比如rot2和trot2

我们可以画出变换后的坐标系

>> plotvol([0 5 0 5])
>> trplot2(T1, 'frame', '1', 'color', 'b')

第一行限定坐标轴的范围，第二行表示画一个坐标系，其标记为 {1} 颜色为蓝色，我们再多画几个坐标系：

>> T2 = transl2(2,1)
T2 =
     1     0     2
     0     1     1
     0     0     1
>> trplot2(T2, 'frame', '2', 'color', 'r')
>> T3 = T1*T2
T3 =
    0.8660   -0.5000    2.2321
    0.5000    0.8660    3.8660
         0         0    1.0000
>> trplot2(T3, 'frame', '3', 'color', 'g')
>> T4 = T2*T1
T4 =
    0.8660   -0.5000    3.0000
    0.5000    0.8660    3.0000
         0         0    1.0000
>> trplot2(T4, 'frame', '4', 'color', 'c')

我们发现坐标系 {3} 和坐标系 {4} 是不同的，这也验证了齐次变换矩阵不可交换。

我们画一个点，世界坐标系下为 $(3,2)$

>> P = [3; 2];
>> plot_point(P, 'label', 'P', 'solid', 'ko');

我们来算算在坐标系 {1} 中，点 P 的坐标是多少。根据公式
$^0p={}^0xi_1cdot{}^1p$稍作变换：
$^1p={}^1xi_0cdot{}^0p=({}^0xi_1)^{-1}cdot{}^0p$用机器人工具箱计算

>> P1 = inv(T1) * [P; 1]
P1 =
    1.7321
   -1.0000
    1.0000

我们首先给点 P 的坐标加了个 1 使其变成齐次坐标，计算得到的结果也是齐次坐标。我们也可以使用工具箱函数这样表示：

>> h2e(inv(T1)*e2h(P))
ans =
    1.7321
   -1.0000

最后得到的结果是欧几里得坐标，函数e2h和h2e是欧几里得坐标和其次坐标之间的转换。

旋转中心

为了进一步说明不可交换性，我们以纯旋转为例

>> plotvol([-5 4 -1 5]);
>> T0 = eye(3,3);
>> trplot2(T0, 'frame', '0');
>> x = transl2(2, 3);
>> trplot2(x, 'frame', 'x');

然后我们创建旋转矩阵，并分别画出与旋转矩阵相乘交换与否的结果

>> R = trot2(2);
>> trplot2(R*x, 'framelabel', 'Rx', 'color', 'r');
>> trplot2(x*R, 'framelabel', 'xR', 'color', 'r');

我们发现坐标系 {Rx} 是绕原点旋转的，而坐标系 {xR} 是绕坐标系 {x} 旋转的。

那如果我们要绕定点旋转呢

>> C = [1 2]';
>> plot_point(C, 'label', 'C', 'solid', 'ko')
>> RC = transl2(C) * R * transl2(-C)
RC =
   -0.4161   -0.9093    3.2347
    0.9093   -0.4161    1.9230
         0         0    1.0000
>> trplot2(RC*x, 'framelabel', 'XC', 'color', 'r');

我们看到坐标系 {x} 确实已绕点 C 旋转。创建所需的变换有些麻烦，而且不是显而易见的。 从右向左读取，我们首先应用原点平移，即从 C 到参考系原点的平移，然后应用围绕该原点的旋转，然后应用反向原点平移，即从参考系原点回到 C 的平移。实现此目的的更具描述性的方法是使用 twist。

twist

通过上一节，我们猜测（以后还会提到），给定任意两坐标系，都可以找到一个旋转中心，将第一个坐标系旋转成第二个坐标系。对于纯平移运动而言，转动中心是无穷远。这就是所谓 twist 背后的关键概念。
当指定了 C 的坐标后我们可以创建一个关于点 C 旋转的 twist

>> tw = Twist('R', C)
tw = 
( 2  -1; 1 )

结果是twist对象，使用两个分量对扭曲矢量进行编码：2矢量矩和1矢量旋转。 第一个参数“ R”表示要计算旋转 twist。 这种特殊的 twist 是单位 twist，因为旋转的大小（twist 的最后一个元素）等于1。

下面创建围绕该单位 twist 旋转 2 弧度的齐次变换矩阵，使用T方法

>> tw.T(2)
ans =
   -0.4161   -0.9093    3.2347
    0.9093   -0.4161    1.9230
         0         0    1.0000

这与前一节计算 RC 的结果相同，但更明确地指定了旋转中心。该中心也被称为pole

>> tw.pole
ans =
     1
     2

如果我们想在 $(1,1)$ 方向上做平移，

>> tw = Twist('T', [1 1])
tw = 
( 0.70711  0.70711; 0 )

我们发现这时 tw 的角度是 0，扭矩的大小为 1。如果我们想平移 $sqrt{2}$ 个单位：

>> tw.T(sqrt(2))
ans =
     1     0     1
     0     1     1
     0     0     1

这不就是之前我们看到的没有旋转，只在x，y方向上哪一栋了一个单位的齐次变换矩阵吗！

对于任意位姿变换，我们有：

>> T = transl2(2,3) * trot2(0.5)
T =
    0.8776   -0.4794    2.0000
    0.4794    0.8776    3.0000
         0         0    1.0000
>> tw = Twist(T)
tw = 
( 2.7082  2.4372; 0.5 )
>> tw.T
ans =
    0.8776   -0.4794    2.0000
    0.4794    0.8776    3.0000
         0         0    1.0000