智能机器人（71）：ROS的TF-transform（01）

PART I ====== 变换矩阵
PART II ====== 欧拉角
PART III ====== 四元数
PART IV ====== 坐标系:空间描述与变换

PART I ====== 变换矩阵

1. 二维的平移操作

一般，可以将平移、旋转、缩放操作用矩阵表示。但是，使用2×2的变换矩阵是没有办法描述平移、旋转变换中的二维平移操作的；同样，3×3矩阵也没法描述三维的平移操作。
所以，为了统一描述二维中的平移、旋转、缩放操作，需要引入3×3矩阵形式；同理，使用4×4的矩阵才能统一描述三维的变换。
因此，用齐次坐标（Homogeneous coordinates）描述点和向量。

对于二维平移，如下图所示：

P点经过x和y方向的平移到P’点，可以得到：
x′=x+tx
y′=y+ty
由于引入了齐次坐标，在描述二维坐标的时候，使用（x，y，w）的方式（一般w=1）。
于是，可以写成下面矩阵形式：
x′=1，0，tx—–x
y′=0，1，ty—–y
w’=0，0，1—–w
也就是说平移矩阵是
1，0，tx
0，1，ty
0，0，1

2. 二维的绕原点旋转操作

首先明确，在二维中，旋转是绕着一个点进行，在三维中，旋转是绕着某一个轴进行。

最简单的，二维中的旋转是绕着坐标原点进行的旋转，如下图所示，点v 绕坐标原点旋转θ 角，得到点v’：

假设，v点的坐标是(x, y) ，那么经过推导可以得到，v’点的坐标（x’, y’)
增加一些中间变量： 设原点到v点的距离为r，设原点到v点的向量与x轴的夹角为ϕ，则有：
x=rcosϕ
y=rsinϕ
和
x′=rcos(θ+ϕ)
y′=rsin(θ+ϕ)
通过三角函数展开得到
x′=rcosθcosϕ−rsinθsinϕ
y′=rsinθcosϕ+rcosθsinϕ
带入x和y的表达，得到：
x′=xcosθ−ysinθ
y′=xsinθ+ycosθ
写成矩阵形式，为：
x′ = [cosθ, -sinθ]—-x
y′ = [sinθ, cosθ]—–y
引入了齐次坐标，扩展为3×3形式，则绕原点旋转的齐次矩阵为：
cosθ, -sinθ，0
sinθ, cosθ，0
0，0，1

3. 二维的绕任意点旋转操作

绕原点旋转，是二维旋转最基本的情况。当需要进行绕任意点旋转时，可以转换到这种基本情况的绕原点旋转，思路如下：
1. 首先将这任意旋转点平移过来，到坐标原点处；
2. 执行最简单的绕坐标原点的旋转；
3. 最后将这任意旋转点平移回去，到原来的位置。
也就是说在处理绕任意点旋转的情况下，需要额外在开头和末尾执行两次平移的操作，中间才是真正的旋转。

假设平移的矩阵是T(x,y)，也就是说我们需要得到的坐标 v’ = T(x,y)*R*T(-x,-y)
（采用列坐标描述点的坐标，因此是左乘：首先执行T(-x,-y)… 依次向左进行。）
这样就很容易得出二维中绕任意点旋转操作的旋转矩阵了，即只需要把三个矩阵乘起来即可：
M=
⎡⎣⎢100010txty1⎤⎦⎥∗⎡⎣⎢cosθsinθ0−sinθcosθ0001⎤⎦⎥∗⎡⎣⎢100010−tx−ty1⎤⎦⎥=⎡⎣⎢cosθsinθ0−sinθcosθ0(1−cosθ)tx+ty∗sinθ(1−cosθ)ty−tx∗sinθ1⎤⎦⎥

对照平移和旋转的矩阵可以看出:
这个3×3矩阵的前2×2部分是和旋转相关的，
第三列是与平移相关。

4. 三维的基本旋转

首先明确，在二维中旋转是绕着一个点进行，在三维中就是绕着某一个轴进行。
三维的方向，可以采用右手坐标系：同时旋转角度的正负，也遵循右手坐标系：如下图所示：

一个三维的旋转，可以转换为绕基本坐标轴的旋转。
因此，首先要讨论一下绕三个坐标值x、y、z的旋转。

4.1. 绕X轴的旋转
在三维中，一个点P(x,y,z)绕x轴旋转θ角，到点P’(x’,y’,z’)。
由于是绕x轴进行的旋转，因此x坐标保持不变，在y和z组成的yoz平面上进行一个二维旋转（y轴类似于二维旋转中的x轴，z轴类似于二维旋转中的y轴），于是降维后有：
x′=x
y′=ycosθ−zsinθ
z′=ysinθ+zcosθ

4.2. 绕Y轴旋转
绕Y轴的旋转和绕X轴的旋转类似，Y坐标保持不变，除Y轴之外，ZOX组成的平面进行一次二维的旋转（Z轴类似于二维旋转的X轴，X轴类似于二维旋转中的Y轴），同样有：
x′=zsinθ+xcosθ
y′=y
z′=zcosθ−xsinθ
注意这里是ZOX，而不是XOZ，观察右手系的图片可以了解到这一点。

4.3. 绕Z轴旋转
与上面类似，绕Z轴旋转，Z坐标保持不变，xoy组成的平面内正好进行一次二维旋转
（和上面旋转的情况完全一样）自由度

4.4. 三维旋转操作总结
可以将绕X、Y和Z坐标轴的旋转矩阵分别记为 Rx(α),Ry(β),Rz(θ)，则有：

5. 三维的绕任意轴旋转

绕任意轴的三维旋转，可以使用类似二维的绕任意点旋转一样，将旋转分解为一些列基本的旋转。
对于点P，绕任意向量u，旋转角度θ，得到点Q，如果已知P点和向量u如何求Q点的坐标？如下图所示：

可以把向量u进行一些旋转，让它与z轴重合，之后旋转P到Q就作了一次绕Z轴的三维基本旋转，然后，再执行反向的旋转，将向量u变回到它原来的方向，也就是说需要进行的操作如下：
1. 将旋转轴u绕x轴旋转至xoz平面
2. 将旋转轴u绕y轴旋转至于z轴重合
3. 绕z轴旋转θ角 ！
4. 执行步骤2的逆过程
5. 执行步骤1的逆过程

5.0. 原始的旋转轴u如下图所示：

5.1. 第1、2、3步骤如下图所示：



5.2.分步解析
步骤1将向量u旋转至xoz平面的操作是一个绕x轴的旋转操作，步骤2将向量u旋转到与z轴重合，
第1、2步骤的示意图如下：

作点P在yoz平面的投影点q，q的坐标是（0, b, c)，原点o与q点的连线oq和z轴的夹角就是u绕x轴旋转的角度。通过这次旋转使得u向量旋转到xoz平面（图中的or向量）
【步骤1】
过r点作z轴的垂线，or与z轴的夹角为β, 这个角度就是绕Y轴旋转的角度，通过这次旋转使得u向量旋转到与z轴重合
【步骤2】

步骤1中绕x轴旋转的是一次基本的绕x轴的三维旋转，旋转矩阵是：（注意α角度是绕x旋转的正向的角度）
旋转矩阵(记作 Rx(α))为:
在完成步骤1之后，向量u被变换到了r的位置，继续步骤2的操作，绕y轴旋转负向的β角，经过这次变换之后向量u与z轴完全重合，这一步也是执行的一次绕Y轴的基本旋转，
旋转矩阵（记作 Ry(−β))为：

在完成前面两个步骤之后，u方向和z轴完全重合，因此执行旋转θ角，执行的是一次绕z轴的基本三维旋转（记作 R(θ)）。

最后两步骤，是前面1和2的逆操作，也就是绕Y轴旋转β，和，绕X轴旋转−α，这两个矩阵分别记作 Ry(β) 和 Rx(−α)。

最终得到绕任意轴u旋转的旋转矩阵是：
M=Rx(−α)Ry(β)Rz(θ)Ry(−β)Rx(α)
（因为使用的列向量，因此执行的是左乘，从右往左）

(注意：上面的(u,v,w)，对应向量(a,b,c) 。)

如果向量是经过单位化的（单位向量），那么有a2+b2+c2=1，可以简化上述的公式，得到：

PART II ====== 欧拉角

6. 欧拉角

上面讨论了绕三条坐标轴旋转的旋转矩阵，旋转矩阵C 一般形式为：
【c11，c21，c31】
【c12，c22.c32】
【c13，c23，c33】
（这里没有用齐次坐标）
(直角坐标系的三个坐标轴方向的单位向量，实际上是一组标准正交基，旋转矩阵C 是一个正交矩阵。
所以， 旋转矩阵表面上看起来有 9 个参数，实际上只有三个是独立的，另外6个有约束。)

该旋转矩阵C 的三个列向量，实际对应着，原坐标系三个坐标轴方向的单位向量在旋转后的新坐标系下的坐标。

为了更直接地指出这三个独立参数，欧拉（Euler）证明了如下事实：
任何一个旋转都可以由连续施行的三次绕轴旋转来实现，这三次绕轴旋转的旋转角就是三个独立参数，称为欧拉角。
欧拉角之所以可以用来描述旋转是来自于欧拉旋转定理：任何一个旋转都可以用三个绕轴旋转的参数来表示。

定义一个欧拉角，需要明确的内容包括：
1. 三个旋转角的组合方式（是xyz还是yzx还是zxy）
2. 旋转角度的参考坐标系统（旋转是相对于固定的坐标系 还是相对于自身的坐标系）
3. 使用旋转角度是左手系还是右手系
4. 三个旋转角的记法
不同人描述的欧拉角的旋转轴和旋转的顺序都可能是不一样的。
当使用其他人提供的欧拉角的实现时，需要首先搞清楚他用的是那种约定。
根据绕轴旋转的顺序不同，欧拉角的表示也不同。

关于描述坐标系{B}相对于参考坐标系{A}的姿态有两种方式。
* 第一种是绕固定（参考）坐标轴旋转：
假设开始两个坐标系重合，先将{B}绕{A}的X轴旋转γ，然后绕{A}的Y轴旋转β，最后绕{A}的Z轴旋转α，就能旋转到当前姿态。
可以称其为X-Y-Z fixed angles或RPY角(Roll, Pitch, Yaw)。
* 另一种姿态描述方式是绕自身坐标轴旋转：
假设开始两个坐标系重合，先将{B}绕自身的Z轴旋转α，然后绕Y轴旋转β，最后绕X轴旋转γ，就能旋转到当前姿态。
称其为Z-Y-X欧拉角，由于是绕自身坐标轴进行旋转。

ROS的TF是前者

6.1 常见的欧拉角表示有 Yaw-Pitch-Roll (Y-X-Z顺序)，通过下面的图片可以形象地进行理解。
Yaw（偏航）：欧拉角向量的y轴
Pitch（俯仰）：欧拉角向量的x轴
Roll（翻滚）： 欧拉角向量的z轴

6.2 ROS的TF-frame里，
采用欧拉角RPY，RPY指的是绕固定坐标系xyz旋转。
Roll（滚转角）Pitch（俯仰角）Yaw（偏航角）分别对应绕XYZ轴旋转。

Roll:横滚

Pitch: 俯仰

Yaw: 偏航

设Roll、Yaw 、Pitch 三个角度分别为 φ、ψ 、θ，那么利用欧拉角进行旋转对应的旋转变换矩阵为：
【cosψ cosθ−sinψ cosφ sinθcosψ sinθ+sinψ cosφ】
【cosθsinψ sinφ−sinψ cosθ−cosψ cosφ sinθ−sinψ sinθ+cosψ 】
【cosφ cosθcosψ sinφsinφ sinθ−sinφ cosθcosφ】
实际上 Roll/Pitch/Yaw的旋转就分别对应着前面给出的旋转矩阵 Rx(),Ry(),Rz()，上面矩阵就是这三个矩阵的复合。

6.3
旋转角的记法
顺序——-飞行器———-望远镜——符号——角速度
第一——heading——-azimuth——–θ——–yaw
第二——attitude——elevation——ϕ——–pitch
第三——bank———–tilt————-ψ——–roll

6.4
欧拉角的好处是简单、容易理解，但使用它作为旋转的工具有严重的缺陷—万向节死锁（Gimbal Lock）。万向节死锁是指物体的两个旋转轴指向同一个方向。
实际上，当两个旋转轴平行时，万向节锁现象发生了，换句话说，绕一个轴旋转可能会覆盖住另一个轴的旋转，从而失去一维自由度。

PART III ====== 四元数

7. 连续的旋转

假设对物体进行一次欧拉角描述的旋转，三个欧拉角分别是（a1，a2，a3）;之后再进行一次旋转，三个欧拉角描述是（b1，b2，b3）;那么能否只用一次旋转（欧拉角描述为（c1，c2，c3）），来达到这两次旋转相同的效果呢？

这样是非常困难的，不能够仅仅使用(a1+b1,a2+b2,b3+b3)来得到这三个角度。
一般来说，需要将欧拉角转换成前面的旋转矩阵或者后面的四元数来进行连续旋转的叠加计算，之后再转换回欧拉角。
但是这样做的次数多了可能会引入很大的误差导致旋转结果出错。比较好的方案是直接使用旋转矩阵或四元数来计算这类问题。

四元数的一个重要应用是用它来描述三维旋转，四元数从某种意义上来说是四维空间的旋转，难以想象，了解它的结论和使用场景更加重要。

7.1
四元数的由来和复数很很大的关系，因此首先讨论一下关于复数的内容。复数中一个比较重要的概念是共轭复数，将复数的虚部取相反数，得到它的共轭复数：
z=a+biz∗=a−bi

当使用i去乘以一个复数时，把得到的结果绘制在复平面上时，发现得到的位置正好是绕原点旋转90度的效果。

于是可以猜测，复数的乘法，和，旋转之间应该有某些关系，例如：
定义一个复数q ， 使用q作为一个旋转的因子
q=cosθ+isinθ

写成矩阵的形式是：
a′–[cosθsinθ]—–[a]
b′–[−sinθcosθ]—-[b]
这个公式正好是二维空间的旋转公式，当把新的到的(a′+b′i)绘制在复平面上时，得到的正好是原来的点(a+bi)旋转θ角之后的位置

7.2
既然使用复数的乘法可以描述二维的旋转，那么拓展一个维度是否能表示三维旋转呢？这个也正是四元数发明者William Hamilton最初的想法，也就是说使用
z=a+ib+jc
i2=j2=−1
但是很遗憾 三维的复数的乘法并不是闭合的。也就是说有可能两个值相乘得到的结果并不是三维的复数。
William Hamilton终于意识到自己所需要的运算在三维空间中是不可能实现的，但在四维空间中是可以的。

四元数是另一种描述三维旋转的方式，四元数使用4个分量来描述旋转，四元数可以写成下面的方式：
q=xi+yj+zk +w
为了方便表示为
q=(x,y,z,w)=(v ,w)
其中v是向量，w是实数。
模为1的四元数称为单位四元数(Unit quaternions)。

注意：
这里四元数的表示形式和齐次坐标一样，但是它们之间没什么关系。
四元数常常用来表示旋转，将其理解为“w表示旋转角度，v表示旋转轴”是错误的，应“w与旋转角度有关，v与旋转轴有关”。

7.3
两四元数相加
A（a+bi+cj+dk） + B(e + fi + gj + hk) = C【 (a+e) + (b+f)i + (c+g)j + (d+h)k 】
两个四元数相减
(sa,va) – (sb,vb) = (sa-sb,va-vb)

7.4 构造四元数
欧拉定理告诉任意三维旋转都可以使用一个旋转向量和旋转角度来描述。因此四元数往往是使用旋转轴和旋转角来构造的：

a）绕向量u，旋转角度θ，构造四元数
u = (ux,uy,uz)=uxi+uyj+uzk
q = exp[θ/2(uxi+uyj+uzk)]
= cosθ/2 + (uxi+uyj+uzk)sinθ/2
所以，可以用一个四元数q=((x,y,z)sinθ/2, cosθ/2) 来执行一个旋转。

或者表述为，
如果有单位四元数 q = (u ⋅sinθ/2, cosθ/2) 的形式，那么该单位四元数可以表示绕轴 u 进行 θ 角的旋转。

b）从一个向量旋转到另一个向量，构造四元数
…

c）从四元数获取旋转矩阵和旋转角
设四元数是q = xi+yj+zk+w，那么旋转角度angle和旋转轴(a,b,c)：
angle = 2 * acos(w)
a = x / sqrt(1-w*w)
b = y / sqrt(1-w*w)
c = z / sqrt(1-w*w)

PART IV ====== 坐标系:空间描述与变换

8. 刚体的姿态描述

刚体在空间中具有6个自由度，因此可以用6个变量描述一个刚体在空间中的位姿：
(x,y,z,alpha,beta,gamma)

这牵扯到两个坐标系：全局坐标系 和 固连在刚体上的本地坐标系。
而对应的坐标变换，也就是指同一空间向量相对于这两个坐标系的坐标之间的相互转换关系。

旋转矩阵与坐标系之间的坐标变换没有任何关系。
旋转矩阵所描述的，是坐标系的旋转运动，也就是怎样把全局坐标系旋转到本地坐标系上去的。
或者说，怎样把一个向量（坐标系其实就是三个向量）旋转到新的位置。
注意，这里说的向量旋转，是发生在一个坐标系下的事情。

坐标系旋转角度θ, 则等同于将目标点围绕坐标原点反方向旋转同样的角度θ。

8.1 可以推导两个坐标系之间坐标变换的方法。

对于空间向量P，在全局坐标系下坐标为(x0,y0,z0)， 在局部坐标系下坐标为(x1,y1,z1)，那么：
P = (i,j,k)(x0,y0,z0) = (i′,j′,k′)(x1,y1,z1) = R∗(i,j,k)(x1,y1,z1) = (i,j,k)∗R(x1,y1,z1)

对比左右两边，可以得出结论：
(x0,y0,z0)T = R∗(x1,y1,z1)T
POS_global = R_g−>l ∗ POS_local

即，点在全局的坐标，等于全局坐标系到局部坐标系的转换矩阵，乘以点在局部的坐标。

8.2 通用的空间描述与变换
更广泛的，有两个坐标系A和B，B坐标系中有一个点P，如何把B坐标系中的P映射到A坐标系呢，这涉及到空间描述与变换。
Ap表示p在A坐标系中的表达，Bp表示p在坐标系B中的表达。Aborig表达B坐标原点orig在A坐标系, 是平移量。
要想将Bp转换为A坐标系中的表达Ap，就需要乘以旋转矩阵R_A->B,是旋转量。
将B坐标系中的p点在A坐标系中进行表达:
Ap = R_A->B * Bp + Aboirg
中间的表达式可以成为旋转算子。
则各旋转矩阵可以里哟写成齐次坐标的形式如下
Ap = M * Bp

即，点在A的坐标，等于A坐标系到B坐标系的转换矩阵，乘以点在B的坐标。

8.3 具体到ROS的tf
有三个坐标系，一个世界坐标系world，已知坐标系known，一个未知坐标系query

要求得未知坐标系在世界坐标系当中的表达，可以用旋转算子来表示：

就说是： 未知坐标系在世界坐标系中的旋转算子 = 未知坐标系在已知坐标系中的旋转算子 * 已知坐标系在世界坐标系中的旋转算子