重要概念
1、位置:相机在空间中的哪个地方
2、姿态:相机的朝向
3、向量:向量是空间中的一个东西,并不与实数发生联系,只有在指定坐标系的情况下,向量才有坐标
4、向量积:
(1)内积是个标量
(2)外积是个向量,方向垂直于两个向量组成的平面
其中 ^ 表示是反对称符号,它将一个向量映射成一个反对称矩阵
例如: 映射为
旋转矩阵
机器人的运动是刚体运动,它保证了同一个向量在不同坐标系下长度和角度都不会发生变化
机器人的相机是一个移动坐标系的原点,它位于一个固定不变的世界坐标系中。相机视野中的向量a在不同坐标系下坐标是不相同的,因此需要坐标变换(欧氏变换),欧氏变换由一个旋转和一个平移组成
先考虑旋转
设一个单位正交基(e1,e2,e3)经过一次旋转变成 (e′1 ,e′2 ,e′3 ),同一个向量在两个坐标系中的坐标分为是 [a1,a2,a3 ] T 和 [a′1,a′2,a′3] T
则
上式同时左乘得到
这里的R就是旋转矩阵,描述旋转本身。旋转矩阵的是一个行列式为1的正交矩阵,反过来,行列式为1的正交矩阵是旋转矩阵
旋转矩阵的集合是特殊正交群,定义如下:
由于旋转矩阵是正交阵,它的逆(也就是转置)描述了一个反向的旋转
考虑平移
一个向量经过一次平移,其结果是原向量+平移向量(记为t)
综上,一个向量a经过一次欧氏变换,得到a′,其数学表达如下:
变换矩阵与齐次坐标
上面欧氏变换的数学表达不是线性关系,经过多次变换,式子会变得复杂
引入齐次坐标后,可使得欧氏变换的数学表达变成线性关系:
,其中T称为变换矩阵,包含了欧氏变换的所有信息(旋转和平移)
变换矩阵的集合是特殊欧氏群,定义如下:
它的逆阵描述一次反向的变换
(后面不再区分齐次坐标和非齐次坐标,见到Ta使用齐次坐标运算,见到Ra使用非齐次坐标运算)
旋转向量
用一个3*3包含9个量的旋转矩阵描述一次只有三个自由度的旋转,用一个4*4包含16个量的变换矩阵描述一次只有六个自由度(分别沿3个轴旋转和平移)的变换,这样的表达方式是冗余的
任意一个旋转都可以用一个旋转轴和一个旋转角来描述,因此可以用一个三维旋转向量(方向与旋转轴一致,长度等于旋转角)描述旋转;类似地,用一个旋转向量和一个平移向量(共六维)描述一次变换
设有一个旋转轴为n,角度为θ的旋转,对应旋转向量为θn。从旋转向量到旋转矩阵的过程由罗德里格斯公式给出:
从旋转矩阵到旋转向量,需要求解θ,n:
,其中R的迹求解公式为
由于旋转轴上的向量经过旋转后不变,因此又如下式子成立:
,这个式子表明n是R特征值1对应的特征向量。求解方程,再归一化,即可得到n
欧拉角
旋转矩阵、旋转向量都能够描述旋转,但是不直观,无法从矩阵和向量中直接看出旋转是怎样的,因此引入欧拉角
欧拉角使用三个分离的转角,将一次旋转分解为三次旋转,直观地描述旋转。旋转的分解方法很多,因此欧拉角也有很多定义方法,最常用的是 偏航-俯仰-滚转(yaw-pitch-roll),它等价于ZYX轴的旋转,其分解过程如下:
1、先绕物体Z轴旋转,得到偏航角yaw
2、再绕旋转后的Y轴旋转,得到俯仰角pitch
3、最后绕旋转后的X轴旋转,得到滚转角roll
欧拉角的缺点是会碰到万向锁问题(奇异性问题),当俯仰角为±90°时,第一次旋转和第三次旋转使用同一个轴,使系统丢失了一个自由度。事实上,通过三个实数描述三维旋转,奇异性问题是不可避免的
四元数
平面上的旋转可以使用单位复数描述;类似地,三维空间中的旋转可以使用单位四元数描述(它既是紧凑的、又是非奇异的)
四元数定义
一个四元数q包含一个实部和三个虚部
其中,i、j、k是三个虚部,满足
四元数的向量形式
其中,s是实部(标量),v是虚部(三维向量)
四元数运算
1、加法:
2、乘法:
3、共轭:
4、模长:
5、逆:
,其中1是四元数
6、数乘:
7、点乘:
旋转的四元数表达
假设某个旋转的旋转轴为,转角为θ,那么这个旋转的四元数表达形式为
反过来也可以从单位四元数计算旋转向量:
从旋转的四元数表达式可以看出,当θ = θ + 2π,得到的是一个相同的旋转,此时四元数变成,因此任意旋转可以由两个互为相反数的四元数表示;当θ = 0,得到一个单位实四元数
设一个三维空间点经过一个由单位四元数q指定的旋转后变为
,则
,其中
,
,
和
中的虚部对应是旋转前后的坐标
四元数与旋转阵、旋转向量的转换
四元数乘法可以用矩阵表达。设,定义符号如下
,则四元数乘法的矩阵表达如下
,同理
回代前面的式子有
,其中
因此,从四元数到旋转阵的表达式为
由旋转矩阵进一步可以得到旋转向量,其中旋转角推导如下(由罗德里格斯公式):
,这里用到倍角公式
对于旋转轴,推导思路跟前面类似
旋转轴上的向量经过旋转后不变,因此取=
,代入式子
,最后得到Rv = v,解方程,归一化,得到旋转轴v
这里进行归一化,实际上就是虚部每个元素同时除以(由
可得,s =
,从三角形关系可以得到向量
的虚部的模是
)
在 旋转的四元数表达 中使用四元数q直接得到旋转向量的方法更为简便!!!