三维空间刚体位姿
三维空间的刚体位姿
参考：《视觉SLAM十四讲》、 四元数和欧拉角
1. 点和向量
向量的内积描述的向量之间的投影关系
向量的外积运算结果为：
a × b =\left[\begin{matrix} a_{1} \\ a_{2} \\ i \end{matrix}\right.\left.\begin{matrix} b_{1} \\ b_{2} \\ j \end{matrix}\right.\left.\begin{matrix} c_{1} \\ c_{2} \\ k \end{matrix}\right] =\left[\begin{matrix} 0&-a_{3}&a_{2}\\ a_{3}&0&-a_{1}\\ -a_{2}&a_{1}&0\end{matrix}\right]b ≜ (a ∧ b)
其模长为 |a||b|\sin{\langle a, b\rangle}；其中楔积符号 \wedge 代表一种反对称操作。
反对称矩阵
设A为n维方阵，若有A’=-A，则称矩阵A为反对称矩阵。对于反对称矩阵，它的主对角线上的元素全为零，而位于主对角线两侧对称的元反号。反对称矩阵具有很多良好的性质，如若A为反对称矩阵，则A’，λA均为反对称矩阵；若A,B均为反对称矩阵，则A±B也为反对称矩阵；设A为反对称矩阵，B为对称矩阵，则AB-BA为对称矩阵；奇数阶反对称矩阵的行列式必为0。反对称矩阵的特征值是0或纯虚数，并且对应于纯虚数的特征向量的实部和虚部形成的实向量等长且互相正交。
外积表示两个向量之间的旋转关系
2. 刚体在欧式空间的位姿
刚性物体在绝大多数情况下属于欧氏空间的情况下进行运动，在这种情况下其运动通常通过旋转和移动来表示；而当涉及到非欧氏空间时，则通常不被涉及。
定义刚体运动,任取 刚体上的两个向量,刚体运动前后(我自己定义的)
- 两个向量的长度不发生改变
- 两个向量之间的夹角不发生改变
2.1 坐标系间的欧式变换(旋转矩阵和位移向量)
对于某一个向量位于两个不同的坐标系{A}和{B}中，并分别具有其对应的坐标表示(a₁, a₂, a₃)与(a₁', a₂', a₃')。以正交基底构成的两个坐标系分别为\left\{\textbf{i}, \textbf{j}, \textbf{k}\right\}与\left\{\textbf{i}', \textbf{j}', \textbf{k}'\right\}。基于此关系，则有以下矩阵变换成立：
[e₁,e₂,e₃] \begin{bmatrix} a₁ \\ a₂ \\ a₃ \end{bmatrix} = [e₁',e₂',e₃'] \begin{bmatrix} a₁' \\ a₂' \\ a₃' \end{bmatrix}
在等式两边左乘 [e_1^T, e_2^T, e_3^T] 后可得
\left[\begin{matrix} a_1\\ a_2\\ a_3\\ \end{matrix}\right]=\left[\begin{matrix} e_1^Te'_1 & e_1^Te'_2 & e_1^Te'_3 \\ e_2^Te'_1 & e_2^Te'_2 & e_2^Te'_3 \\ e_3^Te'_1 & e_3^Te'_2 & e_3^Te'_3 \\ \end{matrix}\right]\left[\begin{matrix} a'_1 \\ a'_2 \\ a'_3 \\ \end{matrix}\right] = R a'
其中该 3×3 矩阵即为旋转矩阵 R。旋转矩阵具有独特的性质：它们是行列式为 1 的正交矩阵
正交矩阵是逆为自身转置的矩阵
我们称旋转矩阵的群为SO(3)，即
SO(3) = \left\{R\in R^{n×n}|RR^T = I, det(R)=1\right\}
其中n代表三维欧式空间。
再加上平移的变换t就得到了
a' = Ra + t
2.2 齐次变换(4*4变换矩阵)
在第2.1节中所述的这种变换并非完全齐次，在应用两次这样的非齐次操作后会产生一定的变化效果。对于连续进行多次这样的非齐素操作而言,过程可能会显得较为繁琐,因此我们采用了一种称为齐次坐标的方法来进行统一表示
T就是我们需要的变换矩阵
除了T还有
- 相似变换T_S
- 仿射变换T_A
- 射影变换T_P
2.3 旋转向量
为什么要用旋转向量?
尽管旋转矩阵R拥有9个元素，但其自由度仅限于3个维度的旋转操作，因此所包含的信息存在多余部分。同时要求该变换必须满足正交性，并且其行列式绝对值为1。
在此基础上，在对优化方案进行调整时应用旋转矩阵确实带来了诸多不便。因此我们采用了一种只需三个参数就能描述旋转的方式
- 其长度为旋转角
- 其方向与旋转轴一致
R = cos\theta I + (1-cos\theta )nn^T+sin\theta n^\land
其中n为旋转轴
\theta = arccos(\frac{tr(R)+1}{2})
旋转向量为\theta n
2.4 欧拉角
向量表示法和矩阵表示法虽然能够描述刚体的旋转运动, 但对人类而言并不直观。为此, 我们引入了另一个概念——欧拉角。
欧拉角有两种形式
一种是围绕固定不变的空间轴线进行旋转变换 假定初始状态下两套坐标系完全重合，在空间中依次将坐标系B绕着A的空间X轴转角_𝛾_ ，随后再绕着A的空间Y轴转角_𝛽_ ，最后再绕着A的空间Z轴转角_𝛼_ ，即可到达当前姿态状态；这种变换方式通常被定义为欧拉角（Euler Angles）中的X-Y-Z顺序转动模式（即Roll-Pitch-Yaw），也可简称为RPY角
- r(roll, 翻滚角) 绕旋转之后的X轴旋转的角度
- p(pitch 俯仰角)绕旋转之后的Y轴旋转的角度
- yaw(偏航角)绕物体的Z轴旋转得到偏航角
此旋转矩阵
R_{XYZ}(α, β, γ)=R_Z(α) R_Y(β) R_X(γ)= \begin{bmatrix} cos α cos β & cos α sin β sin γ - sin α cos γ & cos α sin β cos γ + sin α sin γ \\ sin α cos β & sin α sin β sin γ + cos α cos γ & sin α sin β cos γ - cos α sin γ \\ -sin β & 0 & 1 \end{bmatrix}
在初始状态下绕自身坐标系的Z、Y、X轴依次进行旋转：假设两个坐标系最初重合，在此基础之上先将{B}绕自身坐标系的Z轴旋转 𝛼 ，随后依次绕自身坐标系的Y轴旋转 𝛽 ，最后再绕X轴旋转 𝛾 ，即可完成姿态的描述。这样的角度组合被称为Z-Y-X型欧拉角，并且由于这种变换方式仅涉及对自身坐标系的操作而不依赖于外部参考系的特点而得名。
R_{XYZ'}(α, β, γ) 等于 R_Z(α) \cdot R_Y(β) \cdot R_X(γ) ，其具体形式为：
[R_{XYZ'}] = \begin{bmatrix} cos α \cdot cos β & cos α \cdot sin β \cdot sin γ - sin α \cdot cos γ \\ sin α \cdot cos β & sin α \cdot sin β \cdot sin γ + cos α \cdot cos γ \\ -sin β & cos β \cdot sin γ \end{bmatrix}
通过分析可知，这两种描述方式得出的结果一致。具体而言，在固定坐标轴X-Y-Z上进行的旋转参数为(γ, β, α)，而在自身坐标轴Z-Y-X上的旋转参数为(α, β, γ)。
然而，在表征三维空间中的姿态时
在查阅资料时发现存在两种不同的定义：其中一种是关于RPY角的定义为X-Y-Z顺序，《机器人学》由熊有伦等编著,于机械工业出版社出版；另一种则是Z-Y-X顺序，《视觉SLAM十四讲》中也有相关讨论。
有理论依据表明，在描述三维空间中的旋转时，通常需要三个参数。这表明欧拉角在人机交互场景中存在明显的局限性。
2.5 四元数
旋转矩阵的九个参数存在多余性。欧拉角与旋转向量虽然简洁但会面临奇异性的问题。因此采用四元数来表示物体的旋转。
用表示坐标轴方向的方式而言，在三维空间中任意多次绕坐标轴的旋转都可以被统一地表示为绕某一条固定转轴进行一次特定角度的旋转操作。假设该等效转轴的方向向量为\vec{K}=[k_x,k_y,k_z]^T、相应的等效旋转角大小为\theta、则可以用四元数q=(q_0,q_1,q_2,q_3)=q_0+q_1 i+q_2 j+q_3 k来描述这一变换关系：其中，
\begin{cases} q_1 = k_x \cdot \sin\left( \dfrac{\theta}{2} \right) \\ q_2 = k_y \cdot \sin\left( \dfrac{\theta}{2} \right) \\ q_3 = k_z \cdot \sin\left( \dfrac{\theta}{2} \right) \\ q_0 = \cos\left( \dfrac{\theta}{2} \right) \end{cases}
且有q_0^2+q_1^2+q_2^2+q_3^2=1
相当于一个标量和三个向量, 所以也可以表示为q=[s,v], s=q_0, v=[q_1,q_2,q_3]
根据上面的对应关系我们可以求出旋转轴和旋转角
计算的时候因为向量和点都是三维的所以我们使用p=[0,x,y,z]
则有
p' = qpq^{-1}
3. 李群李代数
3.1 李群
定义:
SO(3) = \left\{R\in R^{3×3}|RR^T = I, det(R)=1\right\}
SE(3) = \left\{T = \left[\begin{matrix} R & t\\ 0^T & 1\\ \end{matrix}\right]\in R^{4×4}|RR^T = I, det(R)=1\right\}
由于变量R受到限制，在加法运算下集合不封闭；然而，在乘法运算下则满足封闭性，并且这样的结构被称为群。
像SO(3)和SE(3)这样的称之为李群