线性代数&群论应用：正逆运动学 & 变换矩阵

在机器人学中，为了描述机器人的运动，将机器人建模为正运动学和逆运动学

正运动学：从机器人的关节空间描述计算笛卡尔空间描述的机器人末端执行器的位置和姿态，该问题通常是一个几何问题，给定一组关节角度，计算末端坐标系相对于基坐标系的位置和姿态。
逆运动学：从笛卡尔空间描述下的机器人末端执行器位置和姿态反算出机器人关节空间应该达到的关节角度组合，是实现机器人控制的一个基本问题。通常因为正运动学方程是非线性的，因此逆运动学问题较为困难，很难得到封闭解，甚至无解。而正运动学的像空间就形成了逆运动学的有解空间，称为机器人的工作空间。

运动学变换不止在机械臂中有使用，在图形学中也有使用，图形学中通过旋转矩阵，平移矩阵，投影矩阵，将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′]

因为基向量之间是正交的且自身又是单位矩向量，所以：

共轭：实部不变，对虚部取逆
取模 ∥ 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).