《Robotics,Vision and Control-2nd Edition》——Learning with Me, day 2

第一章 简要介绍

本书开始从机器人的历史开始介绍，内容较多，有兴趣的可以看机器人领域大牛Oussama Khatib的专著——机器人学手册，这本书内容极为详尽！
有时候一些比较繁琐的内容，我就只看插图。这种方法在鉴别论文的好坏也有用处，一般一篇好的论文，其插图一定极为完美。在这里仅展示一些书中的插图内容。（版权归原书作者所有）下面带大家一起领略以下本书的精美图片。

第二章 位置和方向的表示

其中第二章是本书的基础内容，主要讲坐标系相关的知识点。除了这一章，后面的章节可以选择性的看。
书中对一些公式和公式的推导有很详细的说明，我的博客不在这方面做过多解释。如果有想深入了解的，请看原版书籍中的内容。

1. 首先从二维空间入手

先单独讲矩阵的旋转如何表示，

如果我们要从坐标系V，逆时针旋转 \theta 角度，可由如下转换：

由此可以得到旋转矩阵：

代码例子：

R = rot2(0.2)

运行结果：

R =
0.9801 -0.1987
0.1987 0.9801

（注：以上使用弧度值表示的角度，比如pi表示为3.14）

我们可以验证该旋转矩阵R，矩阵的秩为1（det( R )=1)，R的逆矩阵与R的转置相同。

>> 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

如果我们已知一个旋转矩阵，如何知道旋转的度数？
至少有两个解决方法，一是通过旋转矩阵的定义式，求反三角函数得到角度值；二，可以直接用工具箱自带的函数logm和函数vex，代码如下：

>> R = rot2(0.3)
ans =
0.9553 -0.2955
0.2955 0.9553
>> S = logm(R)
S =
0.0000 -0.3000
0.3000 0.0000
>> vex(S)
ans =
0.3000

关于函数vex的使用方法，可查看MATLAB的帮助；

下面把平移和旋转结合起来，就可以得到两个坐标之间的任意变换。

假设坐标系{V}和坐标系{A}是平行，简单的推导得到坐标系{A}和{B}之间的关系如下：

写成紧凑的形式：

此时综合的变换矩阵为：

下面我们验证一下该矩阵的相关性质：

>> 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(1,2)，表示相对平移坐标（2，1），不旋转；
trot2(30,‘deg’)，表示逆时针旋转30°，不平移。
(注：函数rot2和trot2都可以表示旋转）

直观的用坐标图表示，如下：
运行以下代码：

>> plotvol([0 5 0 5]);
>> trplot2(T1, 'frame', '1', 'color', 'b')
>> 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;
>> trplot2(T4, 'frame', '4', 'color', 'c');

分别运行上述代码，得到如下图像：

其中T3和T4的不同，说明了转换矩阵的左乘次序会影响转换的结果。先平移在旋转，会得到我们直观的结果。如果先旋转，再平移的话，旋转的坐标会影响平移的结果。本篇文章的后面会详细解释这种不可交换性（noncommutativity）

现在我们已经知道各个坐标的平移，那么，坐标中任意一点，在任意一个坐标轴上该如何表示呢？
以上图的大地坐标系中的P(3,2)点为例，它在坐标系{1}中该如何表示？
绘制P点的代码：

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

为了解释变换的方法，我们做一个约定：

等式右边第一部分是向量A到向量B的转换矩阵。
下面做一个简单的推导：


代码表示则为：

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

由于坐标下面多了一个“1”，我们转换一下：

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

（注：函数e2h是从欧几里得坐标转化为同质坐标，具体参考Matlab的help文档。）

举个例子：

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

上述代码绘制坐标系T0（世界坐标系）和X（由T0平移获得）
旋转变换

>> R = trot2(2);

分别绘制经过RX和XR变换的结果（如下图）：

>> trplot2(R*X, 'framelabel', 'RX', 'color', 'r');
>> trplot2(X*R, 'framelabel', 'XR', 'color', 'r');

如果希望坐标轴按照指定点（如图中C点）做平移旋转，如下：

>> 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');

注意其中旋转变换矩阵为RC。这样看上去还是比较复杂，接下来讲一种直观的新方法，利用Twist函数。
首先我们可以明确一个观念，就是两个任意坐标系，可以通过旋转，把一个坐标系和另一个坐标系重合。平移被认为旋转半径无限大。下面来看代码演示：

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

创建一个tw对象，由一个2维向量距和1维向量旋转构成，其中R表示转换方式为旋转（rotate）。

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

用T方法可以得到转换矩阵的具体值，我们看到和之前RC的方法得到值差不多（其实这种方法更加精确）

>> tw.pole'
ans =
1 2

pole方法可以获得旋转中心的点坐标。

如果想要表示平移变换的话，以方向（1,1）为例，看下面的代码：

>> tw = Twist('T', [1 1])
tw =
( 0.70711 0.70711; 0 )
>> tw.T(sqrt(2))
ans =
1 0 1
0 1 1
0 0 1

通过转换矩阵我们可以发现，旋转角度为0，平移方向（1,1）

对于任意平面的变换，例如：

>> T = transl2(2, 3) * trot2(0.5)
T =
0.8776 -0.4794 2.0000
0.4794 0.8776 3.0000
0 0 1.0000

计算其twist向量为：

>> tw = Twist(T)
tw =
( 2.7082 2.4372; 0.5 )

旋转的角度为0.5弧度值。

还原成转换矩阵为：

>> tw.T
ans =
0.8776 -0.4794 2.0000
0.4794 0.8776 3.0000
0 0 1.0000

本节到此告一段落，下一节将把2维平面的结果推广到3维空间。3维空间的结果对于空间机器人（如水下机器人，无人机等）和机械臂来说更加重要！