国外网站打开很慢dns,网站排名英文怎么说,甘特图模板关于网站建设,阿里云ace搭建wordpress三维空间刚体运动4-1#xff1a;四元数表示变换#xff08;各形式相互转换加代码——下篇#xff09; 4. 四元数到其它旋转表示的相互转换4.1 旋转向量4.2 旋转矩阵4.3 欧拉角4.3.1 转换关系4.3.2 转换中的万象锁问题 5. 四元数的其他性质5.1 旋转的复合5.2 双倍覆盖5.3 指数… 三维空间刚体运动4-1四元数表示变换各形式相互转换加代码——下篇 4. 四元数到其它旋转表示的相互转换4.1 旋转向量4.2 旋转矩阵4.3 欧拉角4.3.1 转换关系4.3.2 转换中的万象锁问题 5. 四元数的其他性质5.1 旋转的复合5.2 双倍覆盖5.3 指数形式5.4 幂函数性质 6. 实践6.1 四元数旋转运算6.2 坐标变换 序本篇系列文章参照高翔老师《视觉SLAM十四讲从理论到实践》讲解三维空间刚体运动为读者打下坚实的数学基础。博文将原第三讲分为五部分来讲解其中四元数部分较多较复杂又分为四部分。如果读者急于实践可直接阅读第五部分的机器人运动轨迹此部分详细讲解了安装准备工作。此系列总体目录如下 旋转矩阵和变换矩阵旋转向量表示旋转欧拉角表示旋转四元数包括以下部分 4-1.1 四元数表示变换——上篇 4-1.2 四元数表示变换——下篇 4-2. 四元数线性插值方法LinEuler/LinMat/Lerp/Nlerp/Slerp 4-3. 四元数多点插值方法Squad 4-4. 四元数多点连续解析解插值方法Spicv 4-5. 四元数多点离散数值解插值方法Sping。实践SLAM中显示机器人运动轨迹及相机位姿。
4. 四元数到其它旋转表示的相互转换
任意单位四元数描述了一个旋转该旋转也可用旋转向量、旋转矩阵或欧拉角描述。现在来考察四元数与旋转向量、旋转矩阵以及欧拉角的相互转换关系。
4.1 旋转向量
本节介绍旋转向量与四元数之间的转换关系。 绕坐标轴的多次旋转可以等效为绕某一转轴旋转一定的角度。假设已知等效旋转轴方向单位旋转向量 u u u的坐标为 u [ u x u y u z ] u\begin{bmatrix} u_{x}\\ u_{y}\\ u_{z} \end{bmatrix} u uxuyuz 旋转角为 θ \theta θ。根据3.2.3推导的定理3D旋转公式一般情况四元数型设其等效的单位四元数为 q [ q 0 , q 1 , q 2 , q 3 ] q[q_{0},q_{1},q_{2},q_{3}] q[q0,q1,q2,q3]则有 { q 0 c o s ( 1 2 θ ) q 1 u x s i n ( 1 2 θ ) q 2 u y s i n ( 1 2 θ ) q 3 u z s i n ( 1 2 θ ) (4.1) \left\{\begin{matrix} q_{0}cos(\frac{1}{2}\theta )\\ q_{1}u_{x}sin(\frac{1}{2}\theta )\\ q_{2}u_{y}sin(\frac{1}{2}\theta )\\ q_{3}u_{z}sin(\frac{1}{2}\theta ) \end{matrix}\right.\tag{4.1} ⎩ ⎨ ⎧q0cos(21θ)q1uxsin(21θ)q2uysin(21θ)q3uzsin(21θ)(4.1) 同理当已知单位四元数 q [ q 0 , q 1 , q 2 , q 3 ] q[q_{0},q_{1},q_{2},q_{3}] q[q0,q1,q2,q3]其对应的旋转角 θ \theta θ和旋转向量 u u u为 { θ 2 arccos q 0 [ u x , u y , u z ] T [ q 1 , q 2 , q 3 ] T / sin ( θ 2 ) (4.2) \left\{\begin{matrix} \theta2\arccos q_{0}\\ [u_{x},u_{y},u_{z}]^{T}[q_{1},q_{2},q_{3}]^{T}/\sin(\frac{\theta}{2}) \end{matrix}\right.\tag{4.2} {θ2arccosq0[ux,uy,uz]T[q1,q2,q3]T/sin(2θ)(4.2)
4.2 旋转矩阵
已知单位四元数 q q q根据博文《三维空间刚体运动2旋转向量与罗德里格斯公式》中的罗德里格斯公式展开式2.3及本文公式4.2将单位旋转向量 u u u及旋转角 θ \theta θ替换为单位四元数 q q q省去计算过程得到单位四元数到旋转矩阵的转换公式为 R [ 1 − 2 q 2 2 − 2 q 3 2 2 ( q 1 q 2 − q 0 q 3 ) 2 ( q 1 q 3 q 0 q 2 ) 2 ( q 1 q 2 q 0 q 3 ) 1 − 2 q 1 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 2 q 3 q 0 q 1 ) 1 − 2 q 1 2 − 2 q 2 2 ] (4.3) R\begin{bmatrix} 1-2q_{2}^{2}-2q_{3}^{2} 2(q_{1}q_{2}-q_{0}q_{3}) 2(q_{1}q_{3}q_{0}q_{2})\\ 2(q_{1}q_{2}q_{0}q_{3}) 1-2q_{1}^{2}-2q_{3}^{2} 2(q_{2}q_{3}-q_{0}q_{1})\\ 2(q_{1}q_{3}-q_{0}q_{2}) 2(q_{2}q_{3}q_{0}q_{1}) 1-2q_{1}^{2}-2q_{2}^{2} \end{bmatrix}\tag{4.3} R 1−2q22−2q322(q1q2q0q3)2(q1q3−q0q2)2(q1q2−q0q3)1−2q12−2q322(q2q3q0q1)2(q1q3q0q2)2(q2q3−q0q1)1−2q12−2q22 (4.3) 假设已知变换矩阵 R [ r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 ] R\begin{bmatrix} r_{11} r_{12} r_{13}\\ r_{21} r_{22} r_{23}\\ r_{31} r_{32} r_{33} \end{bmatrix} R r11r21r31r12r22r32r13r23r33 根据公式4.3推导对应的单位四元数为 { q 0 1 2 1 r 11 r 22 r 33 q 1 r 32 − r 23 4 q 0 q 2 r 13 − r 31 4 q 0 q 3 r 21 − r 12 4 q 0 (4.4) \left\{\begin{matrix} q_{0}\frac{1}{2}\sqrt{1r_{11}r_{22}r_{33}}\\ q_{1}\frac{r_{32}-r_{23}}{4q_{0}}\\ q_{2}\frac{r_{13}-r_{31}}{4q_{0}}\\ q_{3}\frac{r_{21}-r_{12}}{4q_{0}} \end{matrix}\right.\tag{4.4} ⎩ ⎨ ⎧q0211r11r22r33 q14q0r32−r23q24q0r13−r31q34q0r21−r12(4.4)
4.3 欧拉角
4.3.1 转换关系
那么将Z-Y-X欧拉角或RPY角绕固定坐标系的X-Y-Z依次旋转α,β,γ角转换为四元数 q [ cos γ 2 0 0 sin γ 2 ] [ cos β 2 0 sin β 2 0 ] [ cos α 2 sin α 2 0 0 ] [ cos α 2 cos β 2 cos γ 2 sin α 2 sin β 2 sin γ 2 sin α 2 cos β 2 cos γ 2 − cos α 2 sin β 2 sin γ 2 cos α 2 sin β 2 cos γ 2 sin α 2 cos β 2 sin γ 2 cos α 2 cos β 2 sin γ 2 − sin α 2 sin β 2 cos γ 2 ] q\begin{bmatrix} \cos\frac{\gamma}{2}\\ 0\\ 0\\ \sin\frac{\gamma}{2} \end{bmatrix} \begin{bmatrix} \cos\frac{\beta}{2}\\ 0\\ \sin\frac{\beta}{2}\\ 0 \end{bmatrix} \begin{bmatrix} \cos\frac{\alpha}{2}\\ \sin\frac{\alpha}{2}\\ 0\\ 0 \end{bmatrix} \begin{bmatrix} \cos\frac{\alpha}{2}\cos\frac{\beta}{2}\cos\frac{\gamma}{2}\sin\frac{\alpha}{2}\sin\frac{\beta}{2}\sin\frac{\gamma}{2}\\ \sin\frac{\alpha}{2}\cos\frac{\beta}{2}\cos\frac{\gamma}{2}-\cos\frac{\alpha}{2}\sin\frac{\beta}{2}\sin\frac{\gamma}{2}\\ \cos\frac{\alpha}{2}\sin\frac{\beta}{2}\cos\frac{\gamma}{2}\sin\frac{\alpha}{2}\cos\frac{\beta}{2}\sin\frac{\gamma}{2}\\ \cos\frac{\alpha}{2}\cos\frac{\beta}{2}\sin\frac{\gamma}{2}-\sin\frac{\alpha}{2}\sin\frac{\beta}{2}\cos\frac{\gamma}{2} \end{bmatrix} q cos2γ00sin2γ cos2β0sin2β0 cos2αsin2α00 cos2αcos2βcos2γsin2αsin2βsin2γsin2αcos2βcos2γ−cos2αsin2βsin2γcos2αsin2βcos2γsin2αcos2βsin2γcos2αcos2βsin2γ−sin2αsin2βcos2γ 根据上面的公式可以求出逆解即由四元数 q ( q 0 , q 1 , q 2 , q 3 ) q(q_{0},q_{1},q_{2},q_{3}) q(q0,q1,q2,q3)到欧拉角的转换为 [ α β γ ] [ a t a n 2 ( 2 ( q 0 q 1 q 2 q 3 ) , 1 − 2 ( q 1 2 q 2 2 ) ) arcsin ( 2 ( q 0 q 2 − q 1 q 3 ) a t a n 2 ( 2 ( q 0 q 3 q 1 q 2 ) , 1 − 2 ( q 2 2 q 3 2 ) ) ] \begin{bmatrix} \alpha\\ \beta\\ \gamma \end{bmatrix} \begin{bmatrix} atan2(2(q_{0}q_{1}q_{2}q_{3}),1-2(q^{2}_{1}q^{2}_{2}))\\ \arcsin (2(q_{0}q_{2}-q_{1}q_{3})\\ atan2(2(q_{0}q_{3}q_{1}q_{2}),1-2(q^{2}_{2}q^{2}_{3})) \end{bmatrix} αβγ atan2(2(q0q1q2q3),1−2(q12q22))arcsin(2(q0q2−q1q3)atan2(2(q0q3q1q2),1−2(q22q32)) 这里使用了 a t a n 2 ( y , x ) atan2(y,x) atan2(y,x)而不是 a r c t a n ( y x ) arctan(\frac{y}{x}) arctan(xy)因为 a r c t a n ( y x ) arctan(\frac{y}{x}) arctan(xy)的取值范围为 [ − π 2 , π 2 ] [-\frac{\pi}{2},\frac{\pi}{2}] [−2π,2π]只有 180 ° 180° 180°而绕某个轴旋转时范围是 360 ° 360° 360° a t a n 2 ( y , x ) atan2(y,x) atan2(y,x)正好满足需求。 a t a n 2 ( y , x ) atan2(y,x) atan2(y,x)是一个函数在C语言里返回的是指方位角函数原型为 d o u b l e a t a n 2 ( d o u b l e y , d o u b l e x ) double \space atan2(double y, double x) double atan2(doubley,doublex) 返回以弧度表示的 y / x y/x y/x 的反正切。y 和 x 的值的符号决定了正确的象限。也可以理解为计算复数 x y i xyi xyi 的辐角计算时atan2 比 atan 稳定。
4.3.2 转换中的万象锁问题
另外需要注意的就是奇异性问题即万向锁下面分析这种情况。当刚体绕Y轴旋转了 90 ° 90° 90°俯仰角pitch90时如何计算横滚角roll和偏航角yaw这时会发生自由度丢失的情况即Yaw和Roll会变为一个自由度。此时再使用上面的公式根据四元数计算欧拉角会出现问题 arcsin ( 2 ( q 0 q 2 − q 1 q 3 ) \arcsin (2(q_{0}q_{2}-q_{1}q_{3}) arcsin(2(q0q2−q1q3)的定义域为 [ − 1 , 1 ] [-1,1] [−1,1]当 2 ( q 0 q 2 − q 1 q 3 ) ± 0.5 2(q_{0}q_{2}-q_{1}q_{3})\pm 0.5 2(q0q2−q1q3)±0.5时在程序中浮点数不能直接进行等于判断要使用合理的阈值俯仰角 β \beta β为 ± 90 ° \pm 90° ±90°将其带入正向公式计算出四元数 ( q 0 , q 1 , q 2 , q 3 ) (q_{0},q_{1},q_{2},q_{3}) (q0,q1,q2,q3)然后可以发现逆向公式中atan2函数中的参数全部为0即出现了 0 0 \frac{0}{0} 00的情况无法计算。 当 β 90 ° \beta90° β90°时 sin β 2 cos β 2 0.707 \sin \frac{\beta}{2}\cos \frac{\beta}{2}0.707 sin2βcos2β0.707将其带入公式中有 q [ q 0 q 1 q 2 q 3 ] [ 0.707 ( cos α 2 cos γ 2 sin α 2 sin γ 2 ) 0.707 ( sin α 2 cos γ 2 − cos α 2 sin γ 2 ) 0.707 ( cos α 2 cos γ 2 sin α 2 sin γ 2 ) 0.707 ( cos α 2 sin γ 2 − sin α 2 cos γ 2 ) ] [ 0.707 cos α − γ 2 0.707 sin α − γ 2 0.707 cos α − γ 2 − 0.707 sin α − γ 2 ] q\begin{bmatrix} q_{0}\\ q_{1}\\ q_{2}\\ q_{3} \end{bmatrix} \begin{bmatrix} 0.707(\cos\frac{\alpha}{2}\cos\frac{\gamma}{2}\sin \frac{\alpha}{2}\sin \frac{\gamma}{2})\\ 0.707(\sin\frac{\alpha}{2}\cos\frac{\gamma}{2}-\cos\frac{\alpha}{2}\sin \frac{\gamma}{2})\\ 0.707(\cos\frac{\alpha}{2}\cos\frac{\gamma}{2}\sin \frac{\alpha}{2}\sin \frac{\gamma}{2})\\ 0.707(\cos\frac{\alpha}{2}\sin\frac{\gamma}{2}-\sin \frac{\alpha}{2}\cos\frac{\gamma}{2}) \end{bmatrix} \begin{bmatrix} 0.707\cos\frac{\alpha - \gamma}{2}\\ 0.707\sin \frac{\alpha - \gamma}{2}\\ 0.707\cos \frac{\alpha - \gamma}{2}\\ -0.707\sin \frac{\alpha - \gamma}{2} \end{bmatrix} q q0q1q2q3 0.707(cos2αcos2γsin2αsin2γ)0.707(sin2αcos2γ−cos2αsin2γ)0.707(cos2αcos2γsin2αsin2γ)0.707(cos2αsin2γ−sin2αcos2γ) 0.707cos2α−γ0.707sin2α−γ0.707cos2α−γ−0.707sin2α−γ 则 q 1 q 0 − q 3 q 2 tan α − γ 2 \frac{q_{1}}{q_{0}}-\frac{q_{3}}{q_{2}}\tan\frac{\alpha - \gamma}{2} q0q1−q2q3tan2α−γ于是有 α − γ 2 a t a n 2 ( q 1 , q 0 ) \alpha - \gamma 2atan2(q_{1},q_{0}) α−γ2atan2(q1,q0)通常令 α 0 \alpha0 α0这时 γ − 2 a t a n 2 ( q 1 , q 0 ) \gamma -2atan2(q_{1},q_{0}) γ−2atan2(q1,q0)。当俯仰角 β \beta β为-90°即刚体竖直向下时的情况也与之类似可以推导出奇异姿态时的计算公式。 将四元数转换为欧拉角的代码可以参考附件。需要注意欧拉角有12种旋转次序而上面推导的公式是按照Z-Y-X顺序进行的所以有时会在网上看到不同的转换公式因为对应着不同的旋转次序在使用时一定要注意旋转次序是什么。比如ADAMS软件里就默认Body 3-1-3次序即Z-X-Z欧拉角而VREP中则按照X-Y-Z欧拉角旋转。另外万向锁问题代码的Z-Y-X欧拉角代码见类CameraSpacePoint。
5. 四元数的其他性质
为更全面理解四元数和方便引入slerp插值这一节补充四元数的其它性质旋转复合、双倍覆盖和指数形式。
5.1 旋转的复合
旋转的复合其实在我们之前证明 q 2 q q [ c o s ( 2 θ ) , s i n ( 2 θ ) u ] q^{2}qq[cos(2\theta),sin(2\theta)u] q2qq[cos(2θ),sin(2θ)u]的时候就已经涉及到一点了但是这里我们考虑的是更一般的情况。 假设有两个表示沿着不同轴、不同角度旋转的四元数 q 1 、 q 2 q_{1}、q_{2} q1、q2。首先我们实施 q 1 q_{1} q1的变换变换之后的 v ′ v^{} v′为 v ′ q 1 v q 1 ∗ v^{}q_{1}vq^{*}_{1} v′q1vq1∗接下来对 v ′ v^{} v′进行 q 2 q_{2} q2的变换得到 v ′ ′ v^{} v′′ v ′ ′ q 2 v ′ q 2 ∗ q 2 q 1 v q 1 ∗ q 2 ∗ q n e t v q n e t ∗ v^{}q_{2}v^{}q^{*}_{2}q_{2}q_{1}vq^{*}_{1}q^{*}_{2}q_{net}vq^{*}_{net} v′′q2v′q2∗q2q1vq1∗q2∗qnetvqnet∗其中 q n e t q 2 q 1 q_{net}q_{2}q_{1} qnetq2q1另外写成上面的形式还用到性质 q 1 ∗ q 2 ∗ ( q 2 q 1 ) ∗ q^{*}_{1}q^{*}_{2}(q_{2}q_{1})^{*} q1∗q2∗(q2q1)∗。 注意四元数乘法的顺序这和矩阵、复数的复合非常相似都是从右向左叠加。另外要注意的是 q 1 q_{1} q1与 q 2 q_{2} q2的等价旋转 q n e t q_{net} qnet并不是沿着 q 1 q_{1} q1与 q 2 q_{2} q2的两个旋转轴进行的两次旋转它是沿着一个全新的旋转轴进行的一个等价旋转仅仅只有旋转的结果相同。
5.2 双倍覆盖
四元数与 3D 旋转的关系并不是一对一的同一个 3D 旋转可以使用两个不同的四元数来表示对任意的单位四元数 q [ c o s ( 1 2 θ ) , s i n ( 1 2 θ ) u ] q[cos(\frac{1}{2}\theta),sin(\frac{1}{2}\theta)u] q[cos(21θ),sin(21θ)u] q q q与 − q −q −q代表的是同一个旋转。如果 q q q表示的是沿着旋转轴 u u u旋转 θ θ θ度那么 − q −q −q代表的是沿着相反的旋转轴 − u −u −u旋转 ( 2 π − θ ) (2π − θ) (2π−θ)负负得正 − q [ − c o s ( 1 2 θ ) , − s i n ( 1 2 θ ) u ] [ c o s ( π − 1 2 θ ) , s i n ( π − 1 2 θ ) ( − u ) ] \begin{aligned} -q [-cos(\frac{1}{2}\theta),-sin(\frac{1}{2}\theta)u] \\ [cos(\pi - \frac{1}{2}\theta),sin(\pi - \frac{1}{2}\theta)(-u)]\end{aligned} −q[−cos(21θ),−sin(21θ)u][cos(π−21θ),sin(π−21θ)(−u)]所以这个四元数旋转的角度为 2 ( π − 1 2 θ ) 2 π − θ 2(\pi - \frac{1}{2}\theta)2\pi-\theta 2(π−21θ)2π−θ从下面的图中我们可以看到这两个旋转是完全等价的 其实从四元数的旋转公式中也能推导出相同的结果 ( − q ) v ( − q ) ∗ ( − 1 ) 2 q v q ∗ q v q ∗ (-q)v(-q)^{*}(-1)^{2}qvq^{*}qvq^{*} (−q)v(−q)∗(−1)2qvq∗qvq∗所以我们经常说单位四元数与3D旋转有一个「2对1满射同态」(2-1 Surjective Homomorphism) 关系或者说单位四元数双倍覆盖(Double Cover)了3D旋转。它的严格证明会用到一些李群的知识这里不再拓展。 有一点需要注意的是虽然 q q q与 − q −q −q是两个不同的四元数但是由于旋转矩阵中的每一项都包含了四元数两个分量的乘积它们的旋转矩阵是完全相同的即旋转矩阵并不会出现双倍覆盖的问题。
5.3 指数形式
在讨论复数的时候我们利用欧拉公式将2D的旋转写成了 v ′ e i θ v v^{}e^{i\theta v} v′eiθv这样的指数形式。实际上我们也可以利用四元数将 3D 旋转写成类似的形式。 类似于复数的欧拉公式四元数也有一个类似的公式。如果 u u u是一个单位向量那么对于单位四元数 u q [ 0 , u ] u_{q} [0, u] uq[0,u]有 e u θ c o s ( θ ) u q s i n ( θ ) c o s ( θ ) u s i n ( θ ) e^{u\theta}cos(\theta)u_{q}sin(\theta)cos(\theta)usin(\theta) euθcos(θ)uqsin(θ)cos(θ)usin(θ)这也就是说 q [ cos ( θ ) , sin ( θ ) u ] q [\cos(\theta), \sin(\theta)u] q[cos(θ),sin(θ)u]可以使用指数表示为 e u θ e^{u\theta} euθ。这个公式的证明与欧拉公式的证明非常类似直接使用级数展开就可以了这里不再扩展。 注意因为 u u u是一个单位向量 u 2 [ − u ⋅ u , 0 ] − ∥ u ∥ 2 − 1 u^{2} [−u · u, 0] −∥u∥^{2} −1 u2[−u⋅u,0]−∥u∥2−1这与欧拉公式中的 i i i是非常类似的。有了指数型的表示方式我们就能够将之前四元数的旋转公式改写为指数形式了由此可得到定义 3D旋转公式指数型任意向量 v v v沿着以单位向量定义的旋转轴 u u u旋转 θ θ θ角度之后的 v ′ v^{} v′可以使用四元数的指数表示。令 w [ 0 , v ] , u q [ 0 , u ] w [0, v], u_{q} [0, u] w[0,v],uq[0,u]那么 w ′ e u q θ 2 v e − u q θ 2 w^{}e^{u_{q}\frac{\theta}{2}}ve^{-u_{q}\frac{\theta}{2}} w′euq2θve−uq2θ有了四元数的指数定义我们就能够定义四元数的更多运算了。首先是自然对数 l o g log log对任意单位四元数 q [ c o s ( θ ) , s i n ( θ ) u ] e u q θ q [cos(θ), sin(θ)u]e^{u_{q}\theta} q[cos(θ),sin(θ)u]euqθ l o g ( q ) l o g ( e u q θ ) [ 0 , u θ ] log(q)log(e^{u_{q}\theta})[0,{u\theta}] log(q)log(euqθ)[0,uθ]接下来是单位四元数的幂运算 q t ( e u q θ ) t e u q ( t θ ) [ c o s ( t θ ) , s i n ( t θ ) u ] q^{t}(e^{u_{q}\theta})^{t}e^{u_{q}(t\theta)}[cos(tθ), sin(tθ)u] qt(euqθ)teuq(tθ)[cos(tθ),sin(tθ)u]可以看到一个单位四元数的 t t t次幂等同于将它的旋转角度缩放至 t t t倍并且不会改变它的旋转轴 u u u必须是单位向量所以一般不能与 t t t结合。这些运算会在之后讨论四元数插值时非常有用。限于篇幅四元数插值的讲解见下一篇博客《三维空间刚体运动4-2四元数插值lerpNlerpSlerpSquad等》
5.4 幂函数性质
上面给出四元数的指数形式后可推导出一个重要的指函数性质这一性质下一篇将用到。首先看推论 四元数推论1 设 q ∈ H 1 , p [ a , b v ] q\in \mathbb{H}_{1},p[a,b\mathbf{v}] q∈H1,p[a,bv]其中 a , b , r ∈ R , v ∈ R 3 a,b,r\in \mathbb{R},\mathbf{v}\in\mathbb{R}^{3} a,b,r∈R,v∈R3如果 q [ r , v ] q ∗ [ r , v ′ ] q[r,\mathbf{v}]q^{*}[r,\mathbf{v^{}}] q[r,v]q∗[r,v′]那么 q [ a , b v ] q ∗ [ a , b v ′ ] q[a,b\mathbf{v}]q^{*}[a,b\mathbf{v^{}}] q[a,bv]q∗[a,bv′]。 推论证明 q p q ∗ q [ a , b v ] q ∗ q b [ a b , v ] q ∗ b [ a b , v ′ ] [ a , b v ′ ] \begin{aligned}qpq^{*}q[a,b\mathbf{v}]q^{*}\\qb[\frac{a}{b},\mathbf{v}]q^{*}\\b[\frac{a}{b},\mathbf{v^{}}]\\[a,b\mathbf{v^{}}]\end{aligned} qpq∗q[a,bv]q∗qb[ba,v]q∗b[ba,v′][a,bv′]根据推论得出以下定理 四元数定理2 设 q , p ∈ H 1 , p [ cos θ , sin θ v ] , t ∈ R q,p\in \mathbb{H}_{1},p[\cos\theta,\sin\theta\mathbf{v}],t\in\mathbb{R} q,p∈H1,p[cosθ,sinθv],t∈R那么 q p t q ∗ ( q p q ∗ ) t qp^{t}q^{*}(qpq^{*})^{t} qptq∗(qpq∗)t。 定理证明 根据推论存在 v ′ ∈ R 3 \mathbf{v^{}}\in\mathbb{R}^{3} v′∈R3使得 q [ cos θ , sin θ v ] q ∗ [ cos θ , sin θ v ′ ] q[\cos\theta,\sin\theta\mathbf{v}]q^{*}[\cos\theta,\sin\theta\mathbf{v^{}}] q[cosθ,sinθv]q∗[cosθ,sinθv′]因此得到 q p t q ∗ q ( exp ( t log p ) ) q ∗ q ( exp ( t [ 0 , θ v ] ) ) q ∗ q ( exp [ 0 , t θ v ] ) q ∗ q ( [ cos t θ , sin t θ v ] ) q ∗ [ cos t θ , sin t θ v ′ ] exp ( t [ 0 , θ v ′ ] ) exp ( t log [ cos θ , sin θ v ′ ] ) exp ( t log ( q p q ∗ ) ) ( q p q ∗ ) t \begin{aligned} qp^{t}q^{*} q(\exp(t\log p))q^{*} \\ q(\exp(t[0,\theta\mathbf{v}]))q^{*} \\ q(\exp[0,t\theta\mathbf{v}])q^{*} \\ q([\cos t\theta,\sin t\theta\mathbf{v}])q^{*} \\ [\cos t\theta,\sin t\theta\mathbf{v^{}}] \\ \exp(t[0,\theta\mathbf{v^{}}]) \\ \exp(t\log[\cos \theta,\sin \theta\mathbf{v^{}}]) \\ \exp(t\log(qpq^{*})) \\ (qpq^{*})^{t}\end{aligned} qptq∗q(exp(tlogp))q∗q(exp(t[0,θv]))q∗q(exp[0,tθv])q∗q([costθ,sintθv])q∗[costθ,sintθv′]exp(t[0,θv′])exp(tlog[cosθ,sinθv′])exp(tlog(qpq∗))(qpq∗)t 至此四元数表示旋转的理论部分讲解完了。
6. 实践
现在我们通过两个小程序实际演练四元数的运算。其中四元数的基础运算和高阶运算程序实现放在下一讲Slerp中。
6.1 四元数旋转运算
第一个小程序是演示四元数的常规运算包括与旋转矩阵、旋转向量的转换以及用四元数旋转一个向量如下所示
#includeiostream
#includecmath
using namespace std;#includeeigen3/Eigen/Core
#includeeigen3/Eigen/Geometryusing namespace Eigen;int main(int argc, char **argv){//Eigen/Geometry模块提供了各种旋转和平移的表示3D旋转矩阵直接使用Matrix3d或Matrix3fMatrix3d rotation_matrix Matrix3d::Identity();//旋转向量使用AngleAxis它底层不直接是Matrix但运算可以当做矩阵因为重载了运算符AngleAxisd rotation_vector(M_PI/4, Vector3d(0, 0, 1));//设置输出精度cout.precision(3);coutrotation matrix \nrotation_matrixendl;coutrotation vector \nrotation_vector.matrix()endl;//旋转向量转换的矩阵可以直接赋值给旋转矩阵rotation_matrix rotation_vector.toRotationMatrix();coutrotation vector to Matrix \nrotation_matrixendl;//旋转向量Vector3d v(1, 0, 0);coutv v.transpose()endl;//四元数可以直接把AngleAxis赋值给四元数反之亦然Quaterniond q Quaterniond(rotation_vector);//coeffs:多项式系数coefficients)其顺序为(x,y,z,w)w为实部xyz为虚部coutquaternion from rotation vector q.coeffs().transpose()endl;//也可以直接把旋转矩阵赋给它q Quaterniond(rotation_matrix);coutquaternion from rotation matrix q.coeffs().transpose()endl;//使用四元数旋转一个向量使用重载的乘法即可//注意q*v在数学上是qvq^{-1}v_rotated q*v;cout(1,0,0) after rotation v_rotated.transpose()endl;//用常规向量乘法表示qvq^{-1}则计算如下Quaterniond q_rotate_v q*Quaterniond(0,1,0,0)*q.inverse();coutshould be equal to q_rotate_v.coeffs().transpose()endl;return 0;输出结果如下
rotation matrix
1 0 0
0 1 0
0 0 1
rotation vector 0.707 -0.707 00.707 0.707 00 0 1
rotation vector to Matrix 0.707 -0.707 00.707 0.707 00 0 1
v 1 0 0
v transofrmed 1.71 3.71 4
quaternion from rotation vector 0 0 0.383 0.924
quaternion from rotation matrix 0 0 0.383 0.924
(1,0,0) after rotation 0.707 0.707 0
should be equal to 0.707 0.707 0 0请读者注意程序代码通常和数学表示有一些细微的差别。例如通过运算符重载四元数和三维向量可以直接计算乘法但在数学上则需要先把向量转成虚四元数再利用四元数乘法进行计算同样的情况也适用于变换矩阵乘三维向量的情况。总体而言程序中的用法会比数学公式更灵活。
6.2 坐标变换
下面我们举一个小例子来演示坐标变换。 例子设有小萝卜一号和小萝卜二号位于世界坐标系中。记世界坐标系为 W W W小萝卜们的坐标系分别为 R 1 R_{1} R1和 R 2 R_{2} R2。小萝卜一号的位姿为 q 1 [ 0.35 , 0.2 , 0.3 , 0.1 ] T q_{1}[0.35,0.2,0.3,0.1]^{T} q1[0.35,0.2,0.3,0.1]T t 1 [ 0.3 , 0.1 , 0.1 ] T t_{1}[0.3,0.1,0.1]^{T} t1[0.3,0.1,0.1]T。小萝卜二号的位姿为 q 2 [ − 0.5 , 0.4 , − 0.1 , 0.2 ] T q_{2}[-0.5,0.4,-0.1,0.2]^{T} q2[−0.5,0.4,−0.1,0.2]T t 2 [ − 0.1 , 0.5 , 0.3 ] T t_{2}[-0.1,0.5,0.3]^{T} t2[−0.1,0.5,0.3]T。这里的 q q q和 t t t表达的是 T R k , W , k 1 , 2 T_{R_{k},W},k1,2 TRk,W,k1,2也就是世界坐标系到相机坐标系的变换关系。现在小萝卜一号看到某个点在自身的坐标系下坐标为 p R 1 [ 0.5 , 0 , 0.2 ] T p_{R_{1}}[0.5,0,0.2]^{T} pR1[0.5,0,0.2]T求该向量在小萝卜二号坐标系下的坐标。 这是一个非常简单但又具有代表性的例子。在实际场景中你经常需要在同一个机器人的不同部分或者不同机器人之间转换坐标。计算过程也很简单只需计算 p R 2 T R 2 , W T W , R 1 p R 1 p_{R_{2}}T_{R_{2},W}T_{W,R_{1}}p_{R_{1}} pR2TR2,WTW,R1pR1下面请看程序
#includeiostream
#includevector
#includealgorithm
#includeeigen3/Eigen/Core
#includeeigen3/Eigen/Geometryusing namespace std;
using namespace Eigen;int main(int argc, char** argv){//位姿四元数q1,q2Quaterniond q1(0.35, 0.2, 0.3, 0.1), q2(-0.5, 0.4, -0.1, 0.2);coutq1.coeffs\nq1.coeffs()endl;coutq2.coeffs\nq2.coeffs()endl;//四元数转换为旋转矩阵coutq1.matrix\nq1.matrix()endl;coutq2.matrix\nq2.matrix()endl;//归一化转换为单位四元数q1.normalize();q2.normalize();coutq1.normalizeq1.matrix()endl;coutq2.normalizeq2.matrix()endl;//位移向量t1,t2,小萝卜一号下的坐标pr1Vector3d t1(0.3, 0.1, 0.1), t2(-0.1, 0.5, 0.3);Vector3d pr1(0.5, 0, 0.2);//Eigen::Isometry3d:欧式变换矩阵4*4Isometry3d Twr1(q1), Tr2w(q2);coutTwr1.matrixTwr1.matrix()endl;coutTr2w.matrixTr2w.matrix()endl;//坐标转换计算TRatTwr1.pretranslate(t1);Tr2w.pretranslate(t2);//先将pr1转换为世界坐标系然后转换为小萝卜二号下的坐标pr2Vector3d pr2 Tr2w * Twr1.inverse()*pr1;coutpr2.transposepr2.transpose()endl;return 0;
}输出结果如下
q1.coeffs0.20.30.1
0.35
q2.coeffs0.4
-0.10.2
-0.5
q1.matrix0.8 0.05 0.250.19 0.9 -0.08
-0.17 0.2 0.74
q2.matrix0.9 0.12 0.26
-0.28 0.6 0.360.06 -0.44 0.66
q1.normalize0.238095 0.190476 0.9523810.72381 0.619048 -0.304762-0.647619 0.761905 0.00952381
q2.normalize0.782609 0.26087 0.565217
-0.608696 0.130435 0.7826090.130435 -0.956522 0.26087
Twr1.matrix0.238095 0.190476 0.952381 00.72381 0.619048 -0.304762 0-0.647619 0.761905 0.00952381 00 0 0 1
Tr2w.matrix0.782609 0.26087 0.565217 0
-0.608696 0.130435 0.782609 00.130435 -0.956522 0.26087 00 0 0 1
pr2.transpose
-0.0309731 0.73499 0.296108四元数的第一部分讲解到此。
本文基于《视觉SLAM十四讲从理论到实践》和《Quaternions, Interpolation and Animation》编写但相对于原文会适当精简同时为便于全面理解会收集其他网络好文根据作者理解加入一些注解和扩展知识点如果您觉得还不错请一键四连点赞关注收藏评论让更多的人看到。
参考文献
《视觉SLAM十四讲从理论到实践》高翔、张涛等著中国工信出版社Quaternions, Interpolation and Animation四元数与三维旋转四元数与欧拉角RPY角的相互转换如何形象地理解四元数
