寒假有几项学习计划，其中有一些是为了一些任务而学，最主要的任务是我要在2021_v4的基础上编写2022_v1的大援代码，为此顺便学习一下机器人学的知识（下学期也有这方面的老黄的课程），看看能不能在结构和算法上对前一版本上有所突破。
本系列参考资料：
- 《Robotics, Vision and Control》
- B站公开课：*交通大学机器人学公开课
- 老黄的《机器人技术基础》课程讲解及PPT
- N多有关机器人学的大佬的博客
讲道理机器人学之所以是机器人学，果然还是有点小难的，我看着这些资料确实有点吃力，看了很久，才把第一章给拿下，希望后面顺利。顺便练习了一下公式该怎么敲（如何打公式确实值得自己单独再学一学。
0128附加：发现公式还挺好打的，本文好多手打公式哦~
00 概述
概述部分我们主要要了解的是我们怎么描述一个物体在空间中的姿态，以及如何变换不同的视角对同一物体进行描述。
有一定基础的可以跳过这个部分。
00-1 点与物体的描述
机器人与计算机视觉中的一个最基本的要求是要表示物体在环境中的位置和方向，这个物体，包括我们将要研究的对象：机器人、摄像机、工件、障碍物和路径。所以对于物体的位置和方向的描述就成为了机器人学的入门知识，也是各种机器人教材、课程的第一课。（hh也是老黄正式上课的第一讲）
现在来想一个问题，对于空间中的一个点（不管是上述哪种物体的抽象），我们该怎么来描述呢？
这个问题很熟悉，我们要采用坐标的形式来描述一个点：即可以用一个坐标向量来表示一个点，代表点在参考坐标系中的位移。
那么我们如何描述一个物体的位置或者姿态呢？
这么问不是很具体，我们可以换一种问法，即，我们如何表示物体上的一个点呢？
这要取决于我们的研究问题，我们通常认为物体是刚性的，构成它的点相对于物体坐标系要保持固定的相对位置，所以很自然的，我们可以对物体建立物体坐标系，用物体坐标系的位置和方向来描述物体的位置和方向。就比如这里的{B}，就是物体坐标系，而{A}是参考坐标系。
00-2 位姿
可以看到，在B坐标系下研究B物体的点更为方便，而B和A之间存在一个向量P的关系，这个向量P就代表了B的位姿。
位姿：物体在坐标系中的位置和方向。图形上表示为一组坐标轴，如坐标系{B}。
我们如果考虑点在这两个坐标系视角的转换，就可以从坐标系的变换入手分析：
对{A}施加一个变换，旋转+平移，就能够将A变换为B，
那么对于坐标系B描述的一个点P的坐标，该怎么用坐标系A来描述？(由于难以手打，所以拍照了)
公式的意思是：P在A下的坐标==B相对于A的位姿 · P在B下的坐标。
00-3 相对位姿
相对于一个参考坐标系A的某个坐标系B的相对位姿用上面的ξ（ksi）表示，描述了坐标系{A}经过平移和旋转转化为{B}的动作。若没有上标A，表示相对于世界坐标系O。
相对位姿的重要特点是可以复合/组合，并通过运算符进行代数计算。
点坐标的换算关系上面已经提到过(这里再提是因为会打这个公式)了：
\[^AP =\;{}^A\xi_B\cdot{}^BP \]下面是位姿的合成关系
\[^A\xi_C = {}^A\xi_B{}\oplus{}^B\xi_A \]所以综合两者，p点从C向A坐标系转换如下：
\[^AP = {}({}^A\xi_B{}\oplus{}^B\xi_C)\cdot{}^CP \]ξ到底是什么呢？
那么在后面的0102部分，我们将介绍这些 ξ 的具体表达。
00-4 抽象一些的代数规则
这里总结一些ξ的代数规则：
-
基本规律
\[\xi{}\oplus{}0=\xi\\ \xi{}\ominus{}0=\xi\\ \xi{}\ominus{}\xi=0\\ \ominus{}\xi{}\oplus{}\xi=0 \] -
逆运算的性质
\[\ominus{}^X\xi_Y = {}^Y\xi_X \] -
不可交换性：
\[\xi_1{}\oplus{}\xi_2\neq{}\xi_2{}\oplus{}\xi_1 \]这一点在后面也有体现，即旋转的不可交换性。
00-5 小总结
- 一个点可以用坐标向量来表示，它代表该点在参考坐标系中的位移；
- 一个刚体可以由其上的一组点代表，方便起见，我们常对物体建立物体坐标系；
- 一个物体在坐标系的位置和方向称为其位姿；
- 相对位姿表示一个坐标系相对另一个坐标系的位姿，ξ；
- 一个点可以用不同坐标系中的不同坐标向量来描述，向量（坐标）之间通过坐标系的相对位姿进行转换，运算符是 · ；
- 用相对位姿写成的代数表达式是可以用线性代数等知识进行运算的。
01 二维空间位姿描述
01-1 概念
对于上图中的两个坐标系，我们如何描述他们之间的关系、或是如何形容两者之间的转换呢？
坐标系{A}和{B}的相对位姿可等价表示为两个坐标系间的平移和旋转的向量叠加。
所以我们可以分别从旋转和平移来讨论其数学表达，最后再叠加起来。
对于上图2.6这个例子，A和B的变换，我们可以先使得B旋转为与A平行的V，再平移。
01-1-1 旋转
就是旋转矩阵：B坐标系变换到V，顺时针旋转θ：
\[^VR_B = \left[\begin{matrix} \cos\theta & -\sin\theta \\ \sin\theta & \cos\theta \end{matrix}\right] \]变换公式为：
\[\left[\begin{matrix} ^Vx \\ ^Vy \end{matrix}\right] = {}^VR_B \left[\begin{matrix} ^Bx \\ ^By \end{matrix}\right] \]点在V下的坐标==BtoV的旋转矩阵×点在B下的坐标.
其实很好推导。不记得了可以再推导一遍。
因为这个旋转矩阵行列式为1，说明向量在变换前后长度不变。
此外该矩阵标准正交（每列都是单位向量且相互正交），而正交矩阵有一个特殊的性质：
\[R^{-1} = R^T \]据此我们可以将上面的公式整理为：
\[\left[\begin{matrix} ^Bx \\ ^By \end{matrix}\right] = {}(^VR_B)^{-1} \left[\begin{matrix} ^Vx \\ ^Vy \end{matrix}\right] = (^VR_B)^T \left[\begin{matrix} ^Vx \\ ^Vy \end{matrix}\right] = (^BR_V) \left[\begin{matrix} ^Vx \\ ^Vy \end{matrix}\right] \]此外由于旋转矩阵的特殊性质，我们还可以得出：
\[R(-\theta) = R(\theta)^T \]
01-1-2 平移
平移相对简单，即进行简单的向量加法即可实现。
下面公式即是对V进行平移(x,y)的动作后得到A的动作公式：
\[\left[\begin{matrix} ^Ax \\ ^Ay \end{matrix}\right] = \left[\begin{matrix} ^Vx \\ ^Vy \end{matrix}\right] + \left[\begin{matrix} x \\ y \end{matrix}\right] \]01-1-3 叠加
叠加只需要我们将上面的Vx、Vy替换为Bx、By即可。
\[\left[\begin{matrix} ^Ax \\ ^Ay \end{matrix}\right] = \left[\begin{matrix} ^Vx \\ ^Vy \end{matrix}\right] + \left[\begin{matrix} x \\ y \end{matrix}\right] = {}^VR_B \left[\begin{matrix} ^Bx \\ ^By \end{matrix}\right] + \left[\begin{matrix} x \\ y \end{matrix}\right] = \left[\begin{matrix} \cos\theta & -\sin\theta & x \\ \sin\theta & \cos\theta & y \end{matrix}\right] \cdot \left[\begin{matrix} ^Bx \\ ^By \\ 1 \end{matrix}\right] \]我们可以把最终结果简记为：
\[^A\tilde{P} = \left[\begin{matrix} ^VR_B & t \\ 0_{1\times2} & 1 \end{matrix}\right] \cdot \left[\begin{matrix} ^Bx \\ ^By \\ 1 \end{matrix}\right] = \left[\begin{matrix} ^VR_B & t \\ 0_{1\times2} & 1 \end{matrix}\right] \cdot {}^B\tilde{P} \]到这里可以很容易看出在这个具体情况下00部分提出的相对位姿ξ是谁。
p～代表坐标向量齐次形式，这在CV领域应用广泛。
\[\tilde{P} = \lambda\tilde{P} \]
01-2 matlab实现
01-2-1 坐标系变换、复合以及不可交换性
% 先用SE2创建一个齐次变换，书上的se2已经更新了
% 9.10版本工具箱中用se2函数，10.4版本为SE2函数
T1 = SE2(1,2,30*pi/180)
% 参数分别是X、Y平移距离、THETA是旋转角度(弧度制)，如果最后注明'deg'则为角度制
% 输出为：
T1 =
0.8660 -0.5000 1
0.5000 0.8660 2
0 0 1
% 创建一个5×5的坐标格子，我们在这个图里绘制一下坐标系>
axis([0 5 0 5]);
% 我们指定这个坐标系标签为1，颜色为蓝色
trplot2(T1, 'frame', '1', 'color', 'b')
% 下面我们用例子来展示一下复合运算的不可交换性
%%创建T2变换及其坐标系
T2 = SE2(2,1,0);
% 输出为：
T2 =
1 0 2
0 1 1
0 0 1
trplot2(T2,'frame','2','color','r')
% 看一看T1T2复合的效果
T3 = T1*T2;
% 输出为：
T3 =
0.8660 -0.5000 2.232
0.5000 0.8660 3.866
0 0 1
trplot2(T3,'frame','3', 'color','g');
% 看一看T2T1复合效果：
T4 = T2*T1;
% 输出为：
T4 =
0.8660 -0.5000 3
0.5000 0.8660 3
0 0 1
trplot2(T4, 'frame', '4', 'color', 'c');
总的输出图像为：
可以看到4和3是不同的。
01-2-2 坐标变换
P = [3 ; 2]
plot_point(P, '*')
P1 = double(inv(T1)) * [P; 1]
% 相对于{1}坐标系的坐标;
% 根据上面概念中的公式推导而来(取一个逆即可)
% 输出为：
P1 = 3×1
1.7321
-1.0000
1.0000
e2h(P)
% 输出为：
ans = 3×1
3
2
1
h2e( double(inv(T1)) * e2h(P) )
% 输出为：
ans = 2×1
1.7321
-1.0000
%下面写法与上面等效
homtrans( double(inv(T1)), P)
% 输出为：
ans = 2×1
1.7321
-1.0000
% 相同的点对于坐标系{2}有：
P2 = homtrans( double(inv(T2)), P)
% 输出为：
P2 = 2×1
1
1
% 此处书中代码落后版本，必须加上double，否则不对应
-
inv() 矩阵求逆
-
e2h()将欧几里得坐标点转换为齐次形式
毕竟欧几里得是Euclid
-
h2e()将齐次形式转换为欧几里得坐标点
02 三维空间位姿描述
02-1 姿态描述概念及方法
三维情况是二维的延伸和升级，三维旋转复杂得多。
不得不提一下欧拉旋转公式：
任何两个独立的正交坐标系都可以通过一系列(不超过3次)相对于坐标轴的旋转得到，其中连续两次的旋转不得绕同一轴线。
此外，旋转顺序也决定旋转结果，这一点可以从00部分的位姿代数ξ不可交换性可以看出。
02-1-1 正交旋转矩阵
概念
我们考虑使用与二维空间类似的ξ来描述三维旋转：
\[\left[\begin{matrix} ^Ax \\ ^Ay \\ ^Az \end{matrix}\right] = {}^AR_B\cdot \left[\begin{matrix} ^Bx \\ ^By \\ ^Bz \end{matrix}\right] \]事实上三维的标准正交旋转矩阵为：与二维有些类似
\[R_x(\theta)= \left[\begin{matrix} 1 & 0 & 0 \\ 0 & \cos\theta & -\sin\theta \\ 0 & \sin\theta & \cos\theta \end{matrix}\right]\\ R_y(\theta)= \left[\begin{matrix} \cos\theta & 0 & -\sin\theta\\ 0 & 1 & 0 \\ \sin\theta & 0 & \cos\theta \end{matrix}\right]\\ R_z(\theta)= \left[\begin{matrix} \cos\theta & -\sin\theta & 0 \\ \sin\theta & \cos\theta & 0\\ 0 & 0 & 1 \end{matrix}\right] \]matlab实现
% Rx(θ)-> rotx()
R = rotx(90)%注意是弧度制
trplot(R1)
%tranimate(R1)%旋转的动画展示
%输出
R1 = 3×3
1 0 0
0 0 -1
0 1 0
%效果是绕x轴逆时针(以下图视角)旋转了90°
%rotx()此MATLAB函数创建一个3×3矩阵，用于旋转3×1矢量或x轴周围矢量的3×N矩阵，单位为ang度。
%⭐⭐一个值得注意的地方：
%动画展示必须要是tranimate(R1)
q = Quaternion(A)
%旋转坐标系绘制
>>q.plot()
%错误绘制方式
>>plot(q)
%旋转动画表示
>>q.animate()
%错误动画表示
>>tranimate(q)
02-1-2 三角度表示法
三角度表示法是一个统称，其中还有很多小的描述方式，比如欧拉式(XYX \ XZX \ YXY \ YZY \ ZXZ \ ZYZ)、卡尔丹式(XYZ \ XZY \ YZX \ YXZ \ ZXY \ ZYX)等，但都满足欧拉旋转公式，这些序列统称为欧拉角。
公式
以上面的ZYZ序列为例：
\[R = R_z(\phi)R_y(\theta)R_z(\varphi) \]matlab实现
R = rotz(0.1/pi*180)*roty(0.2/pi*180)*rotz(0.3/pi*180);
%输出：
R = 3×3
0.9021 -0.3836 0.1977
0.3875 0.9216 0.0198
-0.1898 0.0587 0.9801
% 下面这种方式与上面等价，更为简便
R1 = eul2r(0.1/pi*180, 0.2/pi*180, 0.3/pi*180);
% tr2eul的逆运算是eul2r,获得给定旋转矩阵的欧拉角
gama = tr2eul(R)
%输出：
gama = 1×3
0.1000 0.2000 0.3000
gama1 = tr2eul(R1)
%输出：
gama1 = 1×3
0.1000 0.2000 0.3000
可以看到上面我们的欧拉角选取都是正数，我们可以继续探讨一些特殊情形：
% θ为负时进行逆运算
R = eul2r(0.1/pi*180, -0.2/pi*180, 0.3/pi*180)
%输出：
R = 3×3
0.9021 -0.3836 -0.1977
0.3875 0.9216 -0.0198
0.1898 -0.0587 0.9801
%逆运算：
tr2eul(R)
ans = 1×3
-3.0416 0.2000 -2.8416
可见逆运算结果与正运算输入的参数并不相同，但是前者对应的旋转矩阵仍与后者相同。不同的欧拉角对应同一旋转矩阵，说明：旋转矩阵到欧拉角的映射不唯一。工具箱返回的θ始终为正。
R = eul2r(0.1/pi*180, 0, 0.3/pi*180)
% 输出：
R = 3×3
0.9211 -0.3894 0
0.3894 0.9211 0
0 0 1.0000
tr2eul(R)
% 输出：
ans = 1×3
0 0 0.4000
eul2r(ans)
这与原值完全不同，这是因为θ=0时，我们计算的ZYZ函数实际上成为了：
\[R(\phi)\cdot{}R(\varphi)=R(\phi+\varphi) \]此时我们取逆只能得到两角度的和，而无法具体得知各自为多少。有点像线性代数里三维<->二维作变换时的不可逆情况。
另一种广泛应用的是翻滚-俯仰-偏航角（卡尔丹角）：
\[R = R_x(\theta_r)R_y(\theta_p)R_z(\theta_y) \]也称导航角；这种表示方法允许正负值出现，不会产生多解，但是也存在奇异点(θ=90°)。在工具箱中默认为xyz的顺序。
X：前进方向
Y：右手方向
Z：垂直向下（看具体怎么定义，不唯一）
R = rpy2r(0.1/pi*180,0.2/pi*180,0.3/pi*180)
gamma = tr2rpy(R)
%输出
R = 3×3
0.9363 -0.2751 0.2184
0.2896 0.9564 -0.0370
-0.1987 0.0978 0.9752
gamma = 1×3
0.1000 0.2000 0.3000
02-1-3 奇异点及万向节锁
三角度表示法存在先天不足，因为它存在难以反变换的奇异点。当任意两连续轴共线的时候就会发生。
对于 ZYZ 形式的欧拉角，它发生在 θ=kπ,k∈Z 时，对于用横滚-俯仰-偏航角的情况，会发生在θ=±(2k+1)π/2时。虽然都存在奇异点，但我们可以想办法让奇异点不在航行体正常运行时出现。
我感觉这个知识点有一点难，这个可以等到回头我再积淀积淀再补充。
02-1-4 双向量表示法
对于机械臂末端的末端执行器，我们一般会单独固联一个坐标系{E}；末端轴线为坐标系的 z 轴，并被称为接近向量，记为
\[ \hat{a} = ( a_x , a_y , a_z ) \]根据右手定则，我们至少需要两个确定且正交的向量才能完全确定全部三个单位向量。
所以我们引入姿态向量，它位于末端执行器的两个手指之间，与a正交/垂直。
\[\hat{o} = (o_x,o_y,o_z) \]所以两向量法定义的旋转矩阵如下：
\[R = \left[\begin{matrix} n_x & o_x & a_x \\ n_y & o_y & a_y \\ n_z & o_z & a_z \end{matrix}\right] \]当然根据线性代数的知识可知，任意两非平行向量就可以定义一个坐标系。只不过视我们的需求，我们通常需要将向量调整称为互相垂直的形态。
matlab实现
a = [1 0 0]';%接近向量
o = [0 1 0]';%姿态向量
oa2r(o, a)
% 输出：
ans = 3×3
0 0 1
0 1 0
-1 0 0
02-1-5 绕特定向量旋转
对于空间中两个任意姿态的坐标系，总可以在空间中找到某个轴，使其中一个坐标绕该轴旋转一个角度就能与另一个坐标系姿态重合。
原文这里翻译为任意向量，我觉得特定向量更为合适。
% 拿以前导航角的例子说明：
R = rpy2r(0.1,0.2,0.3)
% 输出：
R = 3×3
1.0000 -0.0052 0.0035
0.0052 1.0000 -0.0017
-0.0035 0.0017 1.0000
% 这个变换确定的旋转角度θ和围绕的特定向量如下
[theta,v] = tr2angvec(R)
%输出：
theta = 0.0065
v = 1×3
0.2660 0.5354 0.8016
% eig()可以求特征向量和特征值
[v,lamda] = eig(R)
% 输出：
v = 3×3 complex
-0.6816 + 0.0000i -0.6816 + 0.0000i 0.2660 + 0.0000i
0.1045 + 0.5880i 0.1045 - 0.5880i 0.5354 + 0.0000i
0.1564 - 0.3927i 0.1564 + 0.3927i 0.8016 + 0.0000i
lamda = 3×3 complex
1.0000 + 0.0065i 0.0000 + 0.0000i 0.0000 + 0.0000i
0.0000 + 0.0000i 1.0000 - 0.0065i 0.0000 + 0.0000i
0.0000 + 0.0000i 0.0000 + 0.0000i 1.0000 + 0.0000i
怎么通过eig()求出的特征向量、特征值确定我们要找的特定向量呢？
我们回忆一下正交旋转矩阵的性质，即一定有一个特征值是实数1，还有一对共轭复特征值：
\[\lambda = \cos\theta\pm{}i\sin\theta \]特征值特征向量的定义为：
\[R\vec{v} = \lambda \vec{v} \]当λ==1时，对应的特征向量时不随旋转而改变的，这就是我们要找的围绕向量轴。如上面的例子，应该是第三列对应的向量。
补充一点：
如果有需求根据这个围绕向量和旋转角度计算旋转矩阵。理论根据是罗德里格斯公式。
R = angvec2r(pi/2, [1,0,0]) % 输出： R = 3×3 1.0000 0 0 0 0.0000 -1.0000 0 1.0000 0.0000[公式](罗德里格斯公式推导 - Jing_Rui - 博客园 (cnblogs.com))如下：S(v)为反对称矩阵
另一种描述是：K为v的单位向量
R=I + (1 - cosΘ)K2 + sinΘ K
最后我们再探讨一点，即这种方法虽然引入了四个参数(三参数的向量轴和单参数的角度)，但实际上可以转化为3个参数：因为单位特征向量可以通过开根号确定第三个值。
02-1-6 单位四元数描述
四元数表示法是对前面三角度表示法的优化，因其格式优雅、功能强大、计算简单而被广泛应用于机器人、计算机视觉、计算机图形学以及航空航天惯性导航领域。
四元数的基本性质：
单位四元数可以描述坐标系的旋转，与02-1-5有一些类似，坐标系绕某单位向量n旋转θ角，用单位四元数可以表示为：
\[0.\quad s = \cos\theta/2\\ \quad \vec{v} = (\sin\theta/2)\vec{n} \] \[1.\quad \dot{q} = s + v = s + v_1i + v_2j + v_3k \] \[2.\quad i^2 = j^2 = k^2 = ijk = -1 \] \[3.\quad\dot {q} = s<v_1,v_2,v_3> \]matlab实现：
-
基本实现：
% 四元数在matlab中通过Quaternion类来实现 q=UnitQuaternion(rpy2tr(0.1 ,0.2,0.3)) %版本原因，需要将Quaternion改为UnitQuaternion，否则会报错 %输出为： q = 0.99999 < 0.00086809, 0.0017476, 0.0026165 > % 为了描述坐标系的旋转，我们使用单位四元数进行描述： q.norm %输出为： ans = 1 %这说明q是一个单位四元数 -
重载一些标准方法、函数：
四元数之间的乘积问题，可以表示为一个矩阵-向量积，如下：
据书可知，这种方法的计算复杂度相对较低。
q = q * q;
%输出为：
q =
0.99998 < 0.0017362, 0.0034952, 0.0052329 >
-
关于取逆
q.inv() %输出为： ans = 0.99998 < -0.0017362, -0.0034952, -0.0052329 > % 可以尝试一下q乘以它的逆： q*q.inv()%或者q/q %输出为： q = 1 < 0, 0, 0 > %这个单位四元数表示无效旋转 -
向正交旋转矩阵转换
q.R %输出为： ans = 3×3 1 0 0 0 1 0 0 0 1 -
绘制四元数所指的方向：
q.plot()可见我们这个旋转确实无效，呈现的还是原来的坐标系。
-
有一个错误或者说困惑：
书上说可以将一个三元向量传入四元数的构造函数中，产生一个单位四元数，现在应该是被删除了吧。这里留有疑惑⭐。
p = UnitQuaternion([1 2 3]) %报错 %而输入 p = UnitQuaternion([1 2 3 5]) %则输出 p = 0.16013 < 0.32026, 0.48038, 0.80064 > -
使用重载的乘法，四元数可用于旋转一个三元向量
p*[1 0 0]' %注意有转置符号’ %输出为： ans = 3×1 -0.7436 0.5641 0.3590
02-2 平移与旋转组合
前面描述的最实用的姿态描述方法是四元数和齐次变换矩阵。
下面我们进行旋转和平移的叠加：
02-2-1 四元数+平移
\[^AP={}^A\xi_B\cdot{}^BP = \dot{q}\cdot{}^BP+t \]P在A下的坐标==表示AtoB旋转的单位四元数·P在B下坐标+平移向量。
02-2-2 齐次矩阵+平移
这个与二维类似，直接写公式。
\[\left[\begin{matrix} ^Ax \\ ^Ay \\ ^Az \\ 1 \end{matrix}\right] = \left[\begin{matrix} ^AR_B & t \\ 0_{1×3} & 1 \end{matrix}\right] \cdot \left[\begin{matrix} ^Bx \\ ^By \\ ^Bz \\ 1 \end{matrix}\right] \]02-2-3 matlab实现
T = transl(1, 0, 0) * trotx(90) *transl(0, 1, 0)
% transl实现平移动作
% 输出：
T = 4×4
1 0 0 1
0 0 -1 0
0 1 0 1
0 0 0 1
% 绘制相应坐标系
trplot(T)
% 要提取矩阵T的旋转矩阵，可用t2r()函数
t2r(T)
%要提取平移向量，可用transl()函数
transl(T)
03 简单总结 | Review
通过本部分的学习，我们需要总结出几个宏观的印象，以便于后续的深入学习：
-
我们认识了很多描述位姿的数学对象，各有优劣，我们需要视情况选择不同的描述方式。
后续主要使用齐次变换的方式。
-
坐标系常见且方便。
-
机器人工具箱中位姿ξ的具体描述方法(表2.1)以及旋转描述法的转换(图2.15)：备忘：