在机器人学中，为了描述机器人的运动，将机器人建模为正运动学和逆运动学
- 正运动学：从机器人的关节空间描述计算笛卡尔空间描述的机器人末端执行器的位置和姿态，该问题通常是一个几何问题，给定一组关节角度，计算末端坐标系相对于基坐标系的位置和姿态。
- 逆运动学：从笛卡尔空间描述下的机器人末端执行器位置和姿态反算出机器人关节空间应该达到的关节角度组合，是实现机器人控制的一个基本问题。通常因为正运动学方程是非线性的，因此逆运动学问题较为困难，很难得到封闭解，甚至无解。而正运动学的像空间就形成了逆运动学的有解空间，称为机器人的工作空间。
运动学变换不止在机械臂中有使用，在图形学中也有使用，图形学中通过旋转矩阵，平移矩阵，投影矩阵，将3D的mesh投影到2D的显示器区域中。
这些变换背后的数学原理都是线性代数和群论。
变换的表示方式
变换的表示分为位置和姿态两部分，各三个自由度，
位置变换没有异议，都是用平移向量 [ x y z ] \begin{bmatrix} x \\ y \\ z \end{bmatrix} xyz 表示。
旋转分为：旋转矩阵，欧拉角，四元数三种表达方式
变换矩阵
旋转矩阵
旋转矩阵的思想是：想要表达姿态，可以用坐标系的三个轴的单位矢量来表示即 [ e 1 e 2 e 3 ] [ x y z ] = [ x e 1 + y e 2 + z e 3 ] \begin{bmatrix} e_1 & e_2 & e_3 \end{bmatrix}\begin{bmatrix} x \\ y \\ z \end{bmatrix} = \begin{bmatrix} xe_1 + ye_2 + ze_3 \end{bmatrix} [e1e2e3] xyz =[xe1+ye2+ze3]，表示旋转就是对这个向量A做变换得到一个新的向量B, R A = B RA = B RA=B，求旋转向量可以从坐标系入手，原坐标系中的XYZ轴，经过旋转得到新的XYZ轴，基向量 [ e 1 e 2 e 3 ] \begin{bmatrix} e_1 & e_2 & e_3 \end{bmatrix} [e1e2e3]乘以旋转矩阵R变成了 [ e 1 ′ e 2 ′ e 3 ′ ] \begin{bmatrix} e_1^{\prime} & e_2^{\prime} & e_3^{\prime} \end{bmatrix} [e1′e2′e3′]：
R [ e 1 e 2 e 3 ] = [ e 1 ′ e 2 ′ e 3 ′ ] R\begin{bmatrix} e_1 & e_2 & e_3 \end{bmatrix}= \begin{bmatrix} e_1^{\prime} & e_2^{\prime} & e_3^{\prime} \end{bmatrix} R[e1e2e3]=[e1′e2′e3′]
因为基向量之间是正交的且自身又是单位矩向量，所以：
[ e 1 T e 2 T e 3 T ] [ e 1 e 2 e 3 ] = I \begin{bmatrix} e_1^T \\ e_2^T \\ e_3^T \end{bmatrix} \begin{bmatrix} e_1 & e_2 & e_3 \end{bmatrix} = I e1Te2Te3T [e1e2e3]=I
所以可以两边同时乘以 [ e 1 T e 2 T e 3 T ] \begin{bmatrix} e_1^T \\ e_2^T \\ e_3^T \end{bmatrix} e1Te2Te3T ,得到R：
R = [ e 1 ′ e 2 ′ e 3 ′ ] [ e 1 T e 2 T e 3 T ] R= \begin{bmatrix} e_1^{\prime} & e_2^{\prime} & e_3^{\prime} \end{bmatrix}\begin{bmatrix} e_1^T \\ e_2^T \\ e_3^T \end{bmatrix} R=[e1′e2′e3′] e1Te2Te3T
R就是两个坐标系的内积，如果只看旋转，即从A坐标系旋转到B坐标系，坐标系原点位置不变。则这种计算方式可以理解为新坐标系B的各轴在原坐标系A的投影，又称为方向余弦矩阵 ：
B A R = [ A x B A y B A z B ] = [ cos ( ∠ X B X A ) cos ( ∠ Y B X A ) cos ( ∠ Z B X A ) cos ( ∠ X B Y A ) cos ( ∠ Y B Y A ) cos ( ∠ Z B Y A ) cos ( ∠ X B Z A ) cos ( ∠ Y B Z A ) cos ( ∠ Z B Z A ) ] { }{B}^{A} R=\left[\begin{array}{llll} { }^{A} x{B} & { }^{A} y_{B} & { }^{A} z_{B} \end{array}\right]=\left[\begin{array}{lll} \cos \left(\angle X_{B} X_{A}\right) & \cos \left(\angle Y_{B} X_{A}\right) & \cos \left(\angle Z_{B} X_{A}\right) \\ \cos \left(\angle X_{B} Y_{A}\right) & \cos \left(\angle Y_{B} Y_{A}\right) & \cos \left(\angle Z_{B} Y_{A}\right) \\ \cos \left(\angle X_{B} Z_{A}\right) & \cos \left(\angle Y_{B} Z_{A}\right) & \cos \left(\angle Z_{B} Z_{A}\right) \end{array}\right] BAR=[AxBAyBAzB]= cos(∠XBXA)cos(∠XBYA)cos(∠XBZA)cos(∠YBXA)cos(∠YBYA)cos(∠YBZA)cos(∠ZBXA)cos(∠ZBYA)cos(∠ZBZA)
当只有一个轴旋转时，例如绕Z轴旋转 α \alpha α度时，则：
R = [ cos α − sin α 0 sin α − cos α 0 0 0 1 ] R = \begin{bmatrix} \cos \alpha & -\sin\alpha & 0\\ \sin \alpha & -\cos \alpha & 0\\ 0 & 0 & 1 \end{bmatrix} R= cosαsinα0−sinα−cosα0001
平移变换时，平移矩阵T为：
T ( t x , t y ) = [ 1 0 0 t x 0 1 0 t y 0 0 1 t z ] A + Y = B T(t_x,t_y) = \begin{bmatrix} 1 & 0 & 0 & t_x \\ 0 & 1 & 0 &t_y \\ 0 & 0 & 1 & t_z \end{bmatrix} \\ A+Y=B T(tx,ty)= 100010001txtytz A+Y=B正好空出来个3*3的位置，为了统一起来，可以扩展一个dimension，让坐标系变为 [ x , y , z , ω ] [x,y,z,\omega] [x,y,z,ω]，其中 ω \omega ω表示放缩，一般都设为1，表示不放缩。这样就可以得到一个4*4的变换矩阵 了，前三列表示旋转，最后一列表示平移，称为齐次坐标系变换 ：
[ x ′ y ′ z ′ 1 ] = [ a b c t x d e f t y g h i t z 0 0 0 1 ] [ x y z 1 ] \begin{bmatrix} x'\\ y' \\ z' \\ 1 \end{bmatrix} = \begin{bmatrix} a & b & c & t_x\\ d & e & f & t_y\\ g & h & i & t_z \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x \\ y \\ z \\ 1 \end{bmatrix} x′y′z′1 = adg0beh0cfi0txtytz1 xyz1
投影矩阵View变换
机器人学里面不会用到，但是在计算机图形学和计算机视觉中会用到，3D的模型要投影到2D中,又被称为视图变换，投影的方式有两种，正交变换和透视变换：
其中透视变换就是相机的工作原理：
这张图是视觉SLAM十四讲里面的图，我感觉那个光心平面应该是镜头而不是"相机"，后面的成像平面就是CMOS感光元件，实际相机里面的CMOS也确实是倒置的，而且左右也是反的。
这里面有几个坐标系，可能CV跟CG的符号不太一样，但是具体原理都是一样的：
- 像素坐标系：其实就是CMOS坐标系，为了区别其他坐标系，把这个坐标系称为 u − v u-v u−v坐标系：左上角是原点，往右是 u u u正方向，往下是 v v v正方向
- 相机坐标系：以相机为原点的局部坐标系
- 世界坐标系：全局坐标系，原点，方向，由用户自行参考决定
相机成像的过程如下：
CG里面是分为了放缩和正交投影两步，因为CG里面需要考虑整个视场内的所有物体，是自己计算出来的。而相机成像只关注三个面：物平面，像平面，像素平面
将物面坐标归一化到成像平面 { x ′ = f z y y ′ = f z x \left\{\begin{matrix} x'=\frac{f}{z}y \\ \\ y'=\frac{f}{z}x \end{matrix}\right. ⎩ ⎨ ⎧x′=zfyy′=zfx，再将成像平面坐标系(图像物理坐标系)变换到像素坐标系 { u = α x ′ + c x v = β y ′ + c y \left\{\begin{array}{l} u=\alpha x^{\prime}+c_{x} \\ v=\beta y^{\prime}+c_{y} \end{array}\right. {u=αx′+cxv=βy′+cy，放缩比例计算方法为CMOS像素点个数/CMOS实际物理尺寸 ，单位为 p i x e l / m pixel/m pixel/m，平移量为CMOS像素点的一半，单位为 p i x e l pixel pixel，将两个联立，得到如下：
z ( u v 1 ) = ( f x 0 c x 0 f y c y 0 0 1 ) ( x y z ) ≜ K P . z\begin{pmatrix}u\\\\v\\\\1\end{pmatrix}=\begin{pmatrix}f_x&0&c_x\\[0.3em]0&f_y&c_y\\[0.3em]0&0&1\end{pmatrix}\begin{pmatrix}x\\[0.3em]y\\[0.3em]z\end{pmatrix}\triangleq\boldsymbol K\boldsymbol P. z uv1 = fx000fy0cxcy1 xyz ≜KP.
这个变换矩阵定义为相机的内参矩阵K，与相机本身结构相关，出厂就确定了，一般是厂家给，也可以自己标定得到。常见标定方法：张正友标定。
进行一般化，将相机坐标和物体坐标经过旋转 R R R和平移 t t t，转化为世界坐标系：
z P u v = z [ u v 1 ] = K ( R P w + t ) = K T P w . z\boldsymbol{P}_{uv}=z\left[\begin{array}{c}u\\\\v\\\\1\end{array}\right]=\boldsymbol{K}\left(\boldsymbol{R}\boldsymbol{P}_w+\boldsymbol{t}\right)=\boldsymbol{K}\boldsymbol{T}\boldsymbol{P}_w. zPuv=z uv1 =K(RPw+t)=KTPw.
其中的变换矩阵T是相机的外参，随着相机的位置变动而变动，将相机固定在机器人上，通过估计相机的外参就可以估计出机器人的运动
相机由于镜头做工和装配工艺，并不完全是线性关系，还存在畸变，分为径向畸变和纵向畸变
桶形畸变是由于图像放大率随着离光轴的距离增加而减小，而枕形畸变却恰好相反。
在这两种畸变中，穿过图像中心和光轴有交点的直线还能保持形状不变。
欧拉角
姿态可以用欧拉角 [ r o l l p i t c h y a w ] \begin{bmatrix} roll \\ pitch \\ yaw \end{bmatrix} rollpitchyaw 来表示。
注意，这里的位置变换和欧拉角都是相对于局部坐标系 x y z xyz xyz自身三轴的转动，不是相对于全局坐标系 X Y Z XYZ XYZ的变换。
相对于世界坐标系的旋转被称为"静态欧拉角"，但是一般是很难得知世界坐标系信息的，更常用的还是绕自身坐标轴旋转
该图是左手坐标系，X叉乘Y需要用左手定则才能得到Z，右手坐标系中是使用右手定则。
优点：直观，易理解，没有限制，任何三个 [ 0 ， 360 ] [0，360] [0，360]的数都是欧拉角
缺点：
- 欧拉角是讲旋分解为绕xyz轴的旋转，这种旋转在部分角度会有多解，所以欧拉角会出现别名问题。当两个轴重合时会出现万向节死锁问题
- 三维空间中，旋转顺序的不同结果是不一样的，先x再y再z的顺序和先z再y再x的顺序结果是不一样的，所以欧拉角是按照顺序来转动的，可以分为rpy(ZYX)顺序以及XYZ等欧拉角顺序。
- 无论哪种都会出现万向节死锁问题，以ZYX为例，如果第二个轴的旋转角为90°倍数 ，那第二次旋转过后的X轴的
绝对方向跟第一次旋转的时候的Z轴的绝对方向是重合的，这样就丢失了一个自由度，第三次旋转即yaw的旋转，跟第一次旋转即roll的旋转是一样的效果。即：动态欧拉角的旋转： [ 20 ° , 90 ° , 30 ° ] [20°,90°,30°] [20°,90°,30°]等同于静态欧拉角中的 [ 50 ° , 90 ° , 0 ° ] [50°,90°,0°] [50°,90°,0°]，且无论怎么改动，绕世界坐标系Z轴的旋转始终为0。 - 静态欧拉角没有万向节死锁问题
- 在欧拉角表示下，想要表示一个旋转，是没办法进行线性插值的，无论是机械臂旋转，还是图形学中物体旋转，都需要进行插值来将旋转这个动作细化为过程加以控制。
- 90度和275度实际位置只差5度，但是插补就会绕个大圈
四元数
四元数可以解决上述问题，四元数的思想为：不分解为三轴的旋转，而是表达出旋转轴和旋转角度.假如一个四元数 ( w , x , y , z ) (w,x,y,z) (w,x,y,z)，旋转轴为 ( x , y , z ) (x,y,z) (x,y,z)，右手法则旋转角度为 w w w.四元数的后三分别表示三个虚轴 i j k ijk ijk，所以四元数又可以写成一个标量和一个向量的组合： [ s , v ] T [s,v]^T [s,v]T，后三个虚轴关系满足: i 2 = j 2 = k 2 = i j k = − 1 i^2=j^2=k^2=ijk=-1 i2=j2=k2=ijk=−1
四元数运算也和普通的向量不大一样：
- 加减法：与普通向量一致。
- 乘法： p q = [ p w q w − p x q x − p y q y − p z q z p w q x + p x q w + p y q z − p z q y p w q y − p x q z + p y q w + p z q x p w q z + p x q y − p y q x + p z q w ] p q=\begin{bmatrix} p_wq_w-p_xq_x-p_yq_y-p_zq_z \\ p_wq_x+p_xq_w+p_yq_z-p_zq_y \\ p_wq_y-p_xq_z+p_yq_w+p_zq_x \\ p_wq_z+p_xq_y-p_yq_x+p_zq_w \end{bmatrix} pq= pwqw−pxqx−pyqy−pzqzpwqx+pxqw+pyqz−pzqypwqy−pxqz+pyqw+pzqxpwqz+pxqy−pyqx+pzqw
类似于矩阵乘法,不满足交换律,但满足结合律和分配律：
p q = [ p w q w − p v T q v p w q v + q w p v + p v × q v ] p q=\begin{bmatrix} p_wq_w-\mathbf p_v^T\mathbf q_v \\ p_w\mathbf q_v+q_w\mathbf p_v+\mathbf p_v\times\mathbf q_v \end{bmatrix} pq=[pwqw−pvTqvpwqv+qwpv+pv×qv] - 共轭：实部不变，对虚部取逆
- 取模 ∥ q ∥ = q ⊗ q ∗ = q ∗ ⊗ q = q w 2 + q x 2 + q y 2 + q z 2 ∥ p ⊗ q ∥ = ∥ q ⊗ p ∥ = ∥ p ∥ ∥ q ∥ \|\mathbf q\|=\sqrt{\mathbf q\otimes\mathbf q^*}=\sqrt{\mathbf q^*\otimes\mathbf q}=\sqrt{q_w^2+q_x^2+q_y^2+q_z^2} \\ \|\mathbf p\otimes \mathbf q\|=\|\mathbf q\otimes \mathbf p\|=\|\mathbf p\|\|\mathbf q\| ∥q∥=q⊗q∗ =q∗⊗q =qw2+qx2+qy2+qz2 ∥p⊗q∥=∥q⊗p∥=∥p∥∥q∥
- 求逆： q − 1 = q ∗ ∥ q ∥ 2 q ⊗ q − 1 = q − 1 ⊗ q = q 1 = [ 1 0 ] \mathbf q^{-1}=\frac{\mathbf q^*}{\|\mathbf q\|^2} \\\mathbf q\otimes\mathbf q^{-1}=\mathbf q^{-1}\otimes\mathbf q=\mathbf q_1=\begin{bmatrix} 1 \\ \mathbf 0 \end{bmatrix} q−1=∥q∥2q∗q⊗q−1=q−1⊗q=q1=[10]
用四元数表示旋转时 ，先将三维点 [ x , y , z ] [x,y,z] [x,y,z]写成虚四元数形式 [ 0 , x , y , z ] [0,x,y,z] [0,x,y,z]，经过四元数 q q q的变换后，结果为：
p ′ = q p q − 1 p^{\prime} = qpq^{-1} p′=qpq−1
Summary
变换本身确定时，表达方式可以互相转换：
四元数->旋转矩阵：
R = [ q 0 2 + q 1 2 − q 2 2 − q 3 2 2 ( q 1 q 2 − q 0 q 3 ) 2 ( q 0 q 2 + q 1 q 3 ) 2 ( q 0 q 3 + q 1 q 2 ) q 0 2 − q 1 2 + q 2 2 − q 3 2 2 ( q 2 q 3 − q 0 q 1 ) 2 ( q 1 q 3 − q 0 q 2 ) 2 ( q 0 q 1 + q 2 q 3 ) q 0 2 − q 1 2 − q 2 2 + q 3 2 ] R=\left[\begin{array}{ccc} q_{0}^{2}+q_{1}^{2}-q_{2}^{2}-q_{3}^{2} & 2\left(q_{1} q_{2}-q_{0} q_{3}\right) & 2\left(q_{0} q_{2}+q_{1} q_{3}\right) \\ 2\left(q_{0} q_{3}+q_{1} q_{2}\right) & q_{0}^{2}-q_{1}^{2}+q_{2}^{2}-q_{3}^{2} & 2\left(q_{2} q_{3}-q_{0} q_{1}\right) \\ 2\left(q_{1} q_{3}-q_{0} q_{2}\right) & 2\left(q_{0} q_{1}+q_{2} q_{3}\right) & q_{0}^{2}-q_{1}^{2}-q_{2}^{2}+q_{3}^{2} \end{array}\right] R= q02+q12−q22−q322(q0q3+q1q2)2(q1q3−q0q2)2(q1q2−q0q3)q02−q12+q22−q322(q0q1+q2q3)2(q0q2+q1q3)2(q2q3−q0q1)q02−q12−q22+q32
旋转矩阵->四元数的转换关系为：
q 0 = 1 2 1 + r 11 + r 22 + r 33 , [ q 1 q 2 q 3 ] = 1 4 q 0 [ r 32 − r 23 r 13 − r 31 r 21 − 2 12 ] \begin{aligned} q_{0} & =\frac{1}{2} \sqrt{1+r_{11}+r_{22}+r_{33}}, \\ {\left[\begin{array}{c} q_{1} \\ q_{2} \\ q_{3} \end{array}\right] } & =\frac{1}{4 q_{0}}\left[\begin{array}{l} r_{32}-r_{23} \\ r_{13}-r_{31} \\ r_{21}-2_{12} \end{array}\right] \end{aligned} q0 q1q2q3 =211+r11+r22+r33 ,=4q01 r32−r23r13−r31r21−212
此外旋转的参考点不同结果也不同：基于固定坐标系的旋转变换左乘旋转矩阵，基于运动坐标系的旋转变换右乘旋转矩阵。
推导参考：旋转矩阵左右乘不同的推导
左乘右乘的区别仅局限于多个旋转时，这里的固定矩阵也不一定是世界坐标系，而是旋转开始时的初始坐标系。运动坐标系指旋转后的坐标系
李群与李代数
群的概念
集合+运算符号。要求满足：
- 封闭性
- 结合律
- 单位元
- 逆
感觉有点像线性代数里面的向量空间的mini版，运算符号的要求变弱了
常见群：
- 一般线性群，可逆矩阵，对矩阵乘法成群。
- 特殊正交群SO(n)：n维旋转矩阵群，可以用来当作旋转矩阵。
- 特殊欧式群SE(n)：n维欧式变换群。
作用
例如研究旋转矩阵，旋转矩阵乘以旋转矩阵还是旋转矩阵，但是加上就不一定了，因此如果用线性代数中向量空间的方法去分析，会有很多都不能用，所以提出了群，研究群的性质。
Ref
GAMES101
GAMES202
视觉SLAM十四讲
Sola, Joan, Jeremie Deray, and Dinesh Atchuthan. "A micro Lie theory for state estimation in robotics." arXiv preprint arXiv:1812.01537 (2018).