本系列文章由 youngpan1101 出品,转载请注明出处。
文章链接: http://blog.csdn.net/youngpan1101/article/details/71086851
作者:宋洋鹏(youngpan1101)
邮箱: yangpeng_song@163.com
该讲详细资料下载链接 【Baidu Yun】【Video】【Code】
若您觉得本博文对您有帮助,请支持高博的新书《视觉SLAM十四讲》,【点击购买】
若您觉得本博文对您有帮助,请支持高博的新书《视觉SLAM十四讲》,【点击购买】
若您觉得本博文对您有帮助,请支持高博的新书《视觉SLAM十四讲》,【点击购买】
三维空间的刚体运动描述方式
旋转向量
- 旋转矩阵表示方法的 缺点
- 旋转矩阵用 9 个量表达了 3 自由度的变换,该表达方式是 冗余 的(编程会浪费存储空间)。
- 旋转矩阵自身为正交矩阵且行列式为 1 的这些 约束 会给矩阵的估计或优化带来困难。
- 旋转向量(亦称轴角,Axis-Angle)表示三维旋转
- 这种表示法只需一个三维向量即可,该表示方法没有约束。
- 旋转轴为 n ,角度为 θ 的旋转对应的旋转向量为
w=θn(3.11)
旋转矩阵 和 旋转向量 的转换关系:
- 旋转向量⇒旋转矩阵 (由罗德里格斯公式(Rodrigues’s Formula)表明):
R=cosθ⋅I+(1−cosθ)nnT+sinθ⋅n∧(3.12)
式 (3.12) 的符号 ∧ 是向量到反对称矩阵的转换符。- 旋转矩阵⇒旋转向量 :
- 对于转角 θ ,有:
tr(R)=cosθ⋅tr(I)+(1−cosθ)tr(nnT)+sinθ⋅tr(n∧)=1+2cosθ(3.13)
所以:
θ=arccos(tr(R)−12)(3.14)- 由于旋转轴上的向量在旋转后不会发生变化,有
Rn=1⋅n(3.15)
所以转轴 n 是矩阵 R 特征值 1 对应的特征向量,求解此方程,再归一化,即可得到旋转轴。- 旋转向量 和 旋转矩阵 只是表达方式不同,表达的东西是一样的。
——————————– 分割线<< 家有小武,如有一母 >>分割线 ——————————–
欧拉角
- 旋转矩阵、旋转向量很难让人直观地知道物体是如何进行旋转的。
- 欧拉角提供了一种非常直观的方式来描述旋转,它使用三个分离的转角,即 将一个旋转分解成三次饶不同轴的旋转 。
- ZYX 转角相当于把任意旋转分解成以下三个轴上的转角
- 绕物体的 Z 轴 旋转,得到 偏航角 yaw
- 绕旋转之后的 Y 轴 旋转,得到 俯仰角 pitch
- 绕旋转之后的 X 轴 旋转,得到 滚转角 roll
yaw-pitch-roll yaw pitch roll 4. 欧拉角最大的缺点:万向锁(Gimbal Lock)
- ZYX 顺序中,若 Pitch 为 正负90度,则第三次旋转和第一次绕同一个轴,使得系统丢失了一个自由度 —— 存在 奇异性 问题。
- 不适于插值和迭代,一般只用于人机交互中。
- 用三个实数来表达三维旋转时,会不可避免地碰到奇异性问题。
- SLAM 程序中很少直接使用欧拉角表达姿态。
——————————– 分割线<< 家有小武,如有一母 >>分割线 ——————————–
四元数
旋转矩阵 欧拉角、旋转向量 四元数 冗余性 紧凑,奇异性 紧凑, 无奇异性
- 四元数是一种扩展的复数(复数可表达二维平面的旋转)。
- 四元数可以表达三维空间旋转。
- 四元数 q 由 一个实部 和 三个虚部 组成:
q=q0+q1i+q2j+q3k(3.16)
式 (3.16) 中 i,j,k 为四元数的三个虚部。虚部满足以下关系:
⎧⎩⎨⎪⎪⎪⎪i2=j2=k2=−1ij=k,ji=−kjk=i,kj=−iki=j,ik=−j(3.17)- 四元数的表示方法:
q=[s,v],s=q0∈R,v=[q1,q2,q3]T∈R3 ( s 为四元数的实部,v 为四元数的虚部)- 四元数的运算
运算名称 公式 加法、减法 qa±qb=[sa±sb,va±vb] 乘法 1) qaqb=sasb−xaxb−yayb−zazb+(saxb+xasb+yazb−zayb)i+(sayb−xazb+yasb+zaxb)j+(sazb+xayb−xbya+zasb)k
2) 写成向量形式: qaqb=[sasb−vTavb,savb+sbva+va×vb]
3) 乘法通常是不可交换的,除非 va和vb 在 R3 中共线,那么外积项为零共轭 1) q∗a=sa−xai−yaj−zak=[sa,−va]
2) q∗q=qq∗=[s2a+vTv,0]模长 1) ||qa||=s2a+x2a+y2a+z2a−−−−−−−−−−−−−−√
2) ||qaqb||=||qa|| ||qb||逆 1) q−1=q∗||q||2
2) 四元数和自身的逆的乘积为实四元数的 1 : qq−1=q−1q=1
3) 如果 q 为单位四元数,逆和共轭就是同一个量
4) 乘积的逆: (qaqb)−1=q−1bq−1a数乘 kq=[ks,kv] 点乘 qa⋅qb=sasb+xaxbi+yaybj+zazbk
- 四元数和角轴的关系
假设某个旋转是绕单位向量 n=[nx,ny,nz]T 进行了角度为 θ 的旋转
- 角轴⇒四元数
q=[cosθ2,nxsinθ2,nysinθ2,nzsinθ2]T(3.18)
式 (3.18) 的 θ 加上 2π ,得到一个相同的旋转,其对应的四元数变成了 −q ,即 任意的旋转都可以由两个互为相反数的四元数表示。- 四元数⇒角轴
{θ=2 arccos q0[nx,ny,nz]T=[q1,q2,q3]T/sinθ2(3.19)- 四元数和旋转矩阵的关系
- 四元数⇒旋转矩阵
设四元数 q=q0+q1i+q2j+q3k ,对应的旋转矩阵为
R=⎡⎣⎢⎢1−2q22−2q232q1q2−2q0q32q1q3+2q0q22q1q2+2q0q31−2q21−2q232q2q3−2q0q12q1q3−2q0q22q2q3+2q0q11−2q21−2q22⎤⎦⎥⎥(3.20)- 旋转矩阵⇒四元数
设旋转矩阵 R={mij},i,j∈[1,2,3] ,对应的四元数为
q0=tr(R)+1−−−−−−−−√2,q1=m23−m324q0,q2=m31−m134q0,q3=m12−m214q0(3.21)- 用四元数旋转一个空间点
将三维空间的点用虚四元数来描述: p=[0,x,y,z]=[0,v] ,
点 p 经过一次以四元数 q 表示的旋转后,得到了 p′ ( p′ 为纯虚四元数):
p′=qpq−1(3.22)
——————————– 分割线<< 家有小武,如有一母 >>分割线 ——————————–
旋转矩阵、欧拉角、四元数比较
任务/性质 | 旋转矩阵 | 欧拉角 | 四元数 |
---|---|---|---|
在坐标系间旋转点 | 能 | 不能(必须转换到矩阵) | 不能(必须转换到矩阵) |
连接或增量旋转 | 能,但经常比四元数慢,小心矩阵蠕变的情况 | 不能 | 能,比矩阵快 |
插值 | 基本上不能 | 能,但可能遭遇万向锁或其他问题 | Slerp 提供了平滑插值 |
易用程度 | 难 | 易 | 难 |
存储内存 | 9个数 | 3个数 | 4个数 |
对给定方位的表达方式是否唯一 | 是 | 不是,有无数多种方法 | 不是,有两种方法,两者为相反数 |
可能导致非法 | 矩阵蠕变 | 任意三个数都能构成合法的欧拉角 | 可能会出现误差积累,从而产生非法的四元数 |
ps: 以上表格引自 旋转矩阵、欧拉角、四元数比较