二维空间位姿描述

二维世界或平面，是我们在高中学习欧几里得几何时就熟悉的。笛卡儿坐标系，或以 x xx 轴和 y yy 轴为正交轴的坐标系，通常绘制成 x轴水平、y轴竖直，两轴的交点称为原点。平行于坐标轴的单位向量用的和表示。一个点用其在 x 轴和 y 轴上的坐标 （x，y）表示，或者写为有界向量：

   （1）
在下图中的一个坐标系 { B }  ，我们希望用参照系 { A } 来描述它。可以清楚地看到，{ B }的原点已被向量t = ( x , y )所取代，然后逆时针旋转一个角度 θ  。因此，位姿的一个具体表示就是三维向量，我们使用符号 ∼ \sim∼ 表示这两种表示是等价的。

遗憾的是，这种表示方法不方便复合，因为：


两边的位姿都是复杂的三角函数。所以，我们将使用种不同的方法来表示旋转。

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

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

根据方程 ( 1 ) 我们可以将点 P 用 { V }  中定义坐标轴的单位向量表示为

（2）

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

坐标系 { B }可以用它的两个正交轴表示，这里用两个单位向量代表：

上式写作矩阵形式为：

（3）

用方程 ( 1 ) 可以在坐标系 { B }  中将 P点表示为

代入方程 ( 3 ) 得

（4）

由于 B p ^Vp都是点 P  的表现形式，现在令方程 ( 2 ) ( 和方程 ( 4 ) 各自右侧相等，可得

上式描述了点如何通过坐标系旋转从坐标系 { B } 变换到坐标系 { V }  。这种类型的矩阵被称为旋转矩阵，记作  这个记号和位置与姿态概述中的 ( 1 )式表述一致）

（5）

旋转矩阵具有一些特殊的属性。 首先，它是正规化的(也称为标准正交)， 因为它的每列都是单位向量且相互正交的。实际上矩阵的每列都是简单地将{ B}定义在{V}中的单位向量，因此根据定义它们都是单位长度且正交的。


其次，它的行列式是+1，这意味着旋转矩阵R属于特殊的二维正交群，或。而且单位行列式还意味着向量在变换前后的长度是不变的，即。


正交矩阵有一种非常方便的属性: ,即它的逆矩阵和转置矩阵相同。因此，我们可以重新将方程(5)整理为

我们注意到，该矩阵的求逆就是将矩阵的上、下标交换位置,并可以得出恒等式 。

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

描述位姿的第二部分是平移。由于坐标系 { V } 和 { A }  的轴是平行的，所以可以简单地进行向量相加：

（6）

或简写为

（7）

其中，t=(x,y) 代表坐标系的平移变换，而坐标系旋转变换用,因为{A}和{V}的轴是平行的,所以。将P点的坐标向量用齐次形式表达为

称为齐次转换矩阵。这个矩阵有一个非常特殊的结构，并且属于特殊的二维欧几里得群，即

与位置与姿态概述中的（1）式比较，显然代表了相对位姿

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计算矩阵的对数

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

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

我们假设二维空间中有个矩阵长这样

这个样子的矩阵叫反对称矩阵（或斜对称矩阵）。用机器人工具箱创建一个反对称矩阵如下：

>> 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)的结果一样。通常我们这样来表示

其中 θ  是旋转角，记号，表示从一个标量映射到反对称矩阵。

齐次变换矩阵
我们传建一个齐次变换矩阵：平移 ( 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表示旋转而不平移。

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

>> 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 的坐标是多少。根据公式

稍作变换

用机器人工具箱计算

>> 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 背后的关键概念。当指定了 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 ) (1,1)(1,1) 方向上做平移，

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

我们发现这时 tw 的角度是 0，扭矩的大小为 1。如果我们想平移 √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