完成7.2.1坐标变换

docs/chapt7/7.2.1空间坐标描述.md

@@ -73,7 +73,8 @@ $$
 {^x}p \\
 {^y}p \\ 
 {^z}p  \\
-\end{bmatrix}
+\end{bmatrix} = [{^x}p，{^y}p , {^z}p  ]^T
+\tag{位置矢量}
 $$
 
 
@@ -107,7 +108,7 @@ $$
 
 我们将上述坐标系{P}的三个轴相对与参考坐标系{A}三个轴的共九个角度的余弦值，组成3*3的矩阵，该矩阵就是旋转矩阵,因该矩阵是{P}相对于{A}之间的姿态关系的表示，故记作${^A_P}R$
 $$
-{^A_P}R=[{^A}x_{P} \ {^A}y_{P} \ {^A}z_{P}] = \begin{bmatrix}{r_{11}}&{r_{12}}&{r_{13}}\\{r_{21}}&{r_{22}}&{r_{23}}\\{r_{31}}&{r_{32}}&{r_{33}}\\\end{bmatrix}
+{^A_P}R=[{^A}x_{P} \ {^A}y_{P} \ {^A}z_{P}] = \begin{bmatrix}{r_{11}}&{r_{12}}&{r_{13}}\\{r_{21}}&{r_{22}}&{r_{23}}\\{r_{31}}&{r_{32}}&{r_{33}}\\\end{bmatrix} \tag{旋转矩阵}
 $$
 
 两个向量的点乘为两个向量的长度与它们夹角余弦的积,所以$r11$可以表示为向量$P_{x}$与$A_{x}$的点积，旋转矩阵就可以写为下面的形式
@@ -136,6 +137,11 @@ $$
 {^A_P}R^T = {^P_A}R = {^A_P}R^{-1}
 $$
 
+需要注意的是：
+
+- 当两个坐标系之间姿态没有变化，即坐标系间$x,y,z$轴方向对应重合，则旋转矩阵为单位矩阵，这个很好求得，有兴趣的同学可以算一下
+- 从矩阵的角度看，矩阵的逆等与矩阵的转置，则该矩阵为正交矩阵，显而易见，旋转矩阵是正交矩阵
+
 ## 4.位置+姿态
 
 通过位置矢量我们可以描述一个点在特定参考坐标系下的位置，通过旋转矩阵可以描述一个点在特定参考坐标系下的姿态。
@@ -157,21 +163,178 @@ $$
 
 **小鱼说：学会了位置和姿态描述，三维空间坐标关系描述相信已经难不倒你了**
 
+## 5.坐标变换
+
+位姿是相对的，同一个物体在不同的参考坐标系下的位姿数据肯定是不同的。在后续的学习和使用当中，我们会经常需要求同一个点在不同坐标系的位姿表示，这就要求我们掌握坐标变换的方法了。
+
+比如在手眼系统中，我们可以通过视觉算法获取到`工件坐标系P`鱼`相机坐标系C`之间的关系，我们想要控制`机械臂的末端`运动到`工件坐标系P`进行夹取，那么我们就要知道`工件坐标系P`在`机器人基坐标系B`下的位姿，如何获取呢？这就需要坐标进行变换了。
+
+![手眼系统的坐标系关系](7.2.1空间坐标描述/imgs/image-20211104102656102.png)
+
+### 5.1 平移坐标变换
+
+![RVIZ平移坐标变换](7.2.1空间坐标描述/imgs/image-20211104110930303.png)
+
+如图，坐标系{A}、{B}、{C}的姿态是相同的，其之间的姿态对应的旋转矩阵都是单位矩阵。在我们已知:
+
+- {A}为参考坐标系，{B}的位置矢量${^A_B}P=[1,1,1]^T$
+- {B}为参考坐标系，{C}的位置矢量${^B_C}P=[3,0,1]^T$
+
+求：
+
+- {A}为参考坐标系，{C}的位置矢量${^A_C}P$
+
+大家可以利用简单的几何知识手算一下，再继续往下看
+
+正确的答案应该是：
+$$
+{^A_C}P=[4,1,2]^T
+$$
+算法也很简单，直接将${^A_B}P$鱼${^B_C}P$相加即可
+$$
+{^A_C}P ={^A_B}P+{^B_C}P=[1,1,1]^T+[3,0,1]^T=[4,1,2]^T
+$$
+由此我们可以得出平移坐标变换可以求出同一个点（{P}）在相同姿态不同位置坐标系({A}鱼{B})下的不同表示,也可以得到坐标的平移方程
+$$
+{^A_C}P ={^A_B}P+{^B_C}P \tag{坐标平移方程}
+$$
+小鱼说需要注意的是：在上述例子中，{A},{B},{C}三个坐标系的姿态都是相同的，所以${^A_C}R={^A_B}R={^B_C}R$，也都是是单位旋转矩阵
+
+### 5.2 旋转坐标变换
+
+如果坐标系之间姿态不同，同一个点的位姿在不同参考姿态下的位置和姿态也会不一样。
+
+![RVIZ旋转坐标变换](7.2.1空间坐标描述/imgs/image-20211104115451563.png)
+
+上图中，坐标系{A}鱼坐标系{B}原点重合（连名字都连一块了），我们已知：
+
+- {B}坐标系绕{A}的z轴即$A_z$旋转了$45^\circ$
+- {B}鱼{C}姿态相同，{B}为参考坐标系，{C}的位置矢量为${^B_C}P=[3,0,1]^T$
+
+求
+
+- {A}为参考坐标系,{C}的位置矢量${^A_C}P$
+- {A}为参考坐标系,{C}的旋转矩阵${^A_C}R$
+
+这次相对来说有一些困难，不过我们依然可以手推出来,接着和小鱼的思路一起走一下
+
+#### 5.2.1 求${^A_C}R$
+
+因为{B}鱼{C}姿态相同，所以{A}参考坐标系下{C}的姿态和{A}参考坐标系下{B}的姿态是相同的，即：
+$$
+{^A_B}R={^A_C}R
+$$
+接着只需要求${^A_B}R$即可，因为{B}坐标系绕{A}的z轴即$A_z$旋转了$45^\circ$,我们可以一一计算旋转矩阵各个元素的值
+$$
+{^A_B}R ={^A_C}R = \begin{bmatrix} 
+{B_{x}\cdot A_x} & {B_{y}\cdot A_x} & {B_{z}\cdot A_x}\\
+{B_{x}\cdot A_y} & {B_{y}\cdot A_y} & {B_{z}\cdot A_y}\\
+{B_{x}\cdot A_z} & {B_{y}\cdot A_z} & {B_{z}\cdot A_z}\\
+\end{bmatrix}
+= \begin{bmatrix} 
+{cos(45)} & cos(90+45) & cos(90)\\
+{cos(90-45)} & cos(45) & cos(90)\\
+{cos(90)} & cos(90) & cos(0)\\
+\end{bmatrix}
+= \begin{bmatrix} 
+{cos(45)} & cos(90+45) & 0\\
+{cos(90-45)} & cos(45) & 0\\
+{0} &0 &1\\
+\end{bmatrix}
+$$
+将结果在出来，再利用[工具](https://www.andre-gaschler.com/rotationconverter/)来算一下，看看最终结果对不对
+
+![绕z轴旋转45度](7.2.1空间坐标描述/imgs/image-20211104150035369.png)
+
+到此我们求出了${^A_C}P$鱼${^A_C}R$，但聪明的鱼粉肯定不会就此罢休，我们接着来举一反三
+
+{B}绕着{A}的z轴旋转了45度我们计算出来了，那如果{B}绕着{A}旋转$\theta$角度该如何呢?相信聪明的鱼粉心中已经有了答案
+$$
+R(z,\theta)= \begin{bmatrix} 
+{cos\theta} & -sin\theta & 0\\
+{sin\theta} & cos\theta & 0\\
+{0} &0 &1\\
+\end{bmatrix} \tag{小鱼提示：重要方程1}
+$$
+那如果不是绕着z轴旋转，而是绕着x轴呢？
+$$
+R(z,\theta)= \begin{bmatrix} 
+1 & 0           &          0\\
+0 & {cos\theta} & -sin\theta \\
+0&{sin\theta} & cos\theta \\
+
+\end{bmatrix} \tag{小鱼提示：重要方程2}
+$$
+绕着y轴呢？
+$$
+R(z,\theta)= \begin{bmatrix} 
+{cos\theta} & 0 & {sin\theta}\\
+0 &1 &0\\
+{-sin\theta} & 0 &cos\theta \\
+
+\end{bmatrix} \tag{小鱼提示：重要方程3}
+$$
+
+#### 5.2.2 求$^A_CP$
+
+现在我们已知$^B_CP$和$^A_BR$,那如何求$^A_CP$呢，我们可以使用下面这个方程
+$$
+^A_CP = {^A_BR}{^B_CP}\tag{坐标旋转方程}
+$$
+关于这个方程，其实是将$^B_CP$在{B}上各轴的分量变换到了{A}的各轴上
+
+${^A_BR}$是一个$3*3$的矩阵，${^B_CP}$是$3*1$的矩阵，其相乘结果还是一个$3*1$的矩阵，利用前几节学习的矩阵乘法手算或者使用numpy进行计算，得到最终结果为：
+$$
+^A_CP = {^A_BR}{^B_CP}\ = [2.12,2.12,1]^T
+$$
+
+> 小思考:{C}的参考坐标系从{B}变成了{A},{C}在空间中的位置发生了变化了吗？
+
+### 5.3平移旋转复合变换
+
+在我们平时搞机（器人）时，一般情况下两个坐标系原点不重合姿态也不相同。我们将坐标变换拆分成先绕参考坐标系旋转，再绕参考坐标系平移两步，这样我们就得到了坐标的复合变换方程
+$$
+^A_CP = {^A_BR}{^B_CP}+^A_BP
+$$
+这个公式其实也很好理解，${^A_BR}{^B_CP}$就是将{C}在{B}上的各轴分量转换到{A}上，再和原来在{A}上各轴的分量再相加。不理解的同学也没事，后面我们使用最多的还是齐次变换矩阵，根据方程直接摝代码就行了。
+
+![复合变换](7.2.1空间坐标描述/imgs/image-20211104160047629.png)
+
+## 6.左手还是右手
+
+最后小鱼要说的是左手坐标系和右手坐标系，一般情况下我们都是使用右手坐标系搞机。
+
+用右手确定的坐标系就是右手坐标系了，跟着小鱼一起做：
+
+拿起你的右手，先给自己竖个大拇指，然后打开手掌，将大拇指的方向朝向下图中的蓝色z轴，让剩下的四根手指朝向红色的x轴，此时朝向手心外的就是绿绿的y轴的方向了。
+
+![右手坐标系](7.2.1空间坐标描述/imgs/image-20211104160841318.png)
+
+如果你换左手，做上述动作，你会发现y轴是朝手心里的。
+
+再放张图给肢体不协调的同学：
+
+![右手坐标系](7.2.1空间坐标描述/imgs/image-20211104162846579.png)
+
+**参考文档**
+
+- 机器人学导论
+- 机器人学基础
+
 
 
 
-5.坐标变换
+--------------
 
-位姿是相对的，同一个物体在不同的参考坐标系下的位姿数据肯定是不同的，在后续的机器人学习和使用当中我们会经常需要获取同一个点在不同坐标系的位姿表示，这就需要我们掌握坐标变换的方法了。
+技术交流&&问题求助：
 
+- **微信公众号及交流群：鱼香ROS**
+- **小鱼微信：AiIotRobot**
+- **QQ交流群：139707339**
 
-5.1 平移坐标变换
+- 版权保护：已加入“维权骑士”（rightknights.com）的版权保护计划
 
-5.2 旋转坐标变换
 
-6.左手还是右手
 
 
 
-参考文档：
-- 机器人学导论