旋转向量4.2

序本篇系列文章参照高翔老师《视觉SLAM十四讲从理论到实践》讲解三维空间刚体运动为读者打下坚实的数学基础。

博文将原第三讲分为五部分来讲解其中四元数部分较多较复杂又分为四部分。

如果读者急于实践可直接阅读第五部分的机器人运动轨迹此部分详细讲解了安装准备工作。

此系列总体目录如下

旋转矩阵和变换矩阵旋转向量表示旋转欧拉角表示旋转四元数包括以下部分

4-1.1

四元数线性插值方法LinEuler/LinMat/Lerp/Nlerp/Slerp

4-3.

四元数多点离散数值解插值方法Sping。

实践SLAM中显示机器人运动轨迹及相机位姿。

任意单位四元数描述了一个旋转该旋转也可用旋转向量、旋转矩阵或欧拉角描述。

现在来考察四元数与旋转向量、旋转矩阵以及欧拉角的相互转换关系。

4.1

绕坐标轴的多次旋转可以等效为绕某一转轴旋转一定的角度。

假设已知等效旋转轴方向单位旋转向量

u\begin{bmatrix}

θ。

根据3.2.3推导的定理3D旋转公式一般情况四元数型设其等效的单位四元数为

q[q0​,q1​,q2​,q3​]则有

q_{1}u_{x}sin(\frac{1}{2}\***ta

)\\

q_{2}u_{y}sin(\frac{1}{2}\***ta

)\\

q_{3}u_{z}sin(\frac{1}{2}\***ta

⎧​q0​cos(21​θ)q1​ux​sin(21​θ)q2​uy​sin(21​θ)q3​uz​sin(21​θ)​(4.1)

同理当已知单位四元数

[u_{x},u_{y},u_{z}]^{T}[q_{1},q_{2},q_{3}]^{T}/\sin(\frac{\***ta}{2})

{θ2arccosq0​[ux​,uy​,uz​]T[q1​,q2​,q3​]T/sin(2θ​)​(4.2)

4.2

q根据博文《三维空间刚体运动2旋转向量与罗德里格斯公式》中的罗德里格斯公式展开式2.3及本文公式4.2将单位旋转向量

u及旋转角

​1−2q22​−2q32​2(q1​q2​q0​q3​)2(q1​q3​−q0​q2​)​2(q1​q2​−q0​q3​)1−2q12​−2q32​2(q2​q3​q0​q1​)​2(q1​q3​q0​q2​)2(q2​q3​−q0​q1​)1−2q12​−2q22​​

​(4.3)

​r11​r21​r31​​r12​r22​r32​​r13​r23​r33​​

(4.4)

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}}

​q1​4q0​r32​−r23​​q2​4q0​r13​−r31​​q3​4q0​r21​−r12​​​(4.4)

4.3

那么将Z-Y-X欧拉角或RPY角绕固定坐标系的X-Y-Z依次旋转α,β,γ角转换为四元数

cos

\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}

​cos2α​cos2β​cos2γ​sin2α​sin2β​sin2γ​sin2α​cos2β​cos2γ​−cos2α​sin2β​sin2γ​cos2α​sin2β​cos2γ​sin2α​cos2β​sin2γ​cos2α​cos2β​sin2γ​−sin2α​sin2β​cos2γ​​

arcsin

atan2(2(q_{0}q_{1}q_{2}q_{3}),1-2(q^{2}_{1}q^{2}_{2}))\\

\arcsin

atan2(2(q_{0}q_{3}q_{1}q_{2}),1-2(q^{2}_{2}q^{2}_{3}))

\end{bmatrix}

​atan2(2(q0​q1​q2​q3​),1−2(q12​q22​))arcsin(2(q0​q2​−q1​q3​)atan2(2(q0​q3​q1​q2​),1−2(q22​q32​))​

atan2(y,x)

atan2(y,x)是一个函数在C语言里返回的是指方位角函数原型为

double

另外需要注意的就是奇异性问题即万向锁下面分析这种情况。

当刚体绕Y轴旋转了

90°

90°俯仰角pitch90时如何计算横滚角roll和偏航角yaw这时会发生自由度丢失的情况即Yaw和Roll会变为一个自由度。

此时再使用上面的公式根据四元数计算欧拉角会出现问题

arcsin

2(q0​q2​−q1​q3​)±0.5时在程序中浮点数不能直接进行等于判断要使用合理的阈值俯仰角

\beta

(q0​,q1​,q2​,q3​)然后可以发现逆向公式中atan2函数中的参数全部为0即出现了

\frac{0}{0}

0.707(\cos\frac{\alpha}{2}\cos\frac{\gamma}{2}\sin

\frac{\alpha}{2}\sin

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

0.707(\cos\frac{\alpha}{2}\sin\frac{\gamma}{2}-\sin

\frac{\alpha}{2}\cos\frac{\gamma}{2})

\end{bmatrix}

​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α−γ​​

tan

\frac{q_{1}}{q_{0}}-\frac{q_{3}}{q_{2}}\tan\frac{\alpha

\gamma}{2}

β为-90°即刚体竖直向下时的情况也与之类似可以推导出奇异姿态时的计算公式。

将四元数转换为欧拉角的代码可以参考附件。

需要注意欧拉角有12种旋转次序而上面推导的公式是按照Z-Y-X顺序进行的所以有时会在网上看到不同的转换公式因为对应着不同的旋转次序在使用时一定要注意旋转次序是什么。

比如ADAMS软件里就默认Body

3-1-3次序即Z-X-Z欧拉角而VREP中则按照X-Y-Z欧拉角旋转。

另外万向锁问题代码的Z-Y-X欧拉角代码见类CameraSpacePoint。

为更全面理解四元数和方便引入slerp插值这一节补充四元数的其它性质旋转复合、双倍覆盖和指数形式。

5.1

q^{2}qq[cos(2\***ta),sin(2\***ta)u]

q2qq[cos(2θ),sin(2θ)u]的时候就已经涉及到一点了但是这里我们考虑的是更一般的情况。

q_{1}、q_{2}

v^{}q_{2}v^{}q^{*}_{2}q_{2}q_{1}vq^{*}_{1}q^{*}_{2}q_{net}vq^{*}_{net}

v′′q2​v′q2∗​q2​q1​vq1∗​q2∗​qnet​vqnet∗​其中

q_{net}q_{2}q_{1}

q^{*}_{1}q^{*}_{2}(q_{2}q_{1})^{*}

注意四元数乘法的顺序这和矩阵、复数的复合非常相似都是从右向左叠加。

另外要注意的是

q_{1}

q2​的两个旋转轴进行的两次旋转它是沿着一个全新的旋转轴进行的一个等价旋转仅仅只有旋转的结果相同。

5.2

q[cos(\frac{1}{2}\***ta),sin(\frac{1}{2}\***ta)u]

u旋转

[-cos(\frac{1}{2}\***ta),-sin(\frac{1}{2}\***ta)u]

[cos(\pi

\frac{1}{2}\***ta)(-u)]\end{aligned}

−q​[−cos(21​θ),−sin(21​θ)u][cos(π−21​θ),sin(π−21​θ)(−u)]​所以这个四元数旋转的角度为

2(\pi

2(π−21​θ)2π−θ从下面的图中我们可以看到这两个旋转是完全等价的

(-q)v(-q)^{*}(-1)^{2}qvq^{*}qvq^{*}

(−q)v(−q)∗(−1)2qvq∗qvq∗所以我们经常说单位四元数与3D旋转有一个「2对1满射同态」(2-1

Surjective

Cover)了3D旋转。

它的严格证明会用到一些李群的知识这里不再拓展。

−q是两个不同的四元数但是由于旋转矩阵中的每一项都包含了四元数两个分量的乘积它们的旋转矩阵是完全相同的即旋转矩阵并不会出现双倍覆盖的问题。

5.3

e^{u\***ta}cos(\***ta)u_{q}sin(\***ta)cos(\***ta)usin(\***ta)

euθcos(θ)uq​sin(θ)cos(θ)usin(θ)这也就是说

cos

euθ。

这个公式的证明与欧拉公式的证明非常类似直接使用级数展开就可以了这里不再扩展。

注意因为

i是非常类似的。

有了指数型的表示方式我们就能够将之前四元数的旋转公式改写为指数形式了由此可得到定义

3D旋转公式指数型任意向量

w^{}e^{u_{q}\frac{\***ta}{2}}ve^{-u_{q}\frac{\***ta}{2}}

w′euq​2θ​ve−uq​2θ​有了四元数的指数定义我们就能够定义四元数的更多运算了。

首先是自然对数

log

log(q)log(e^{u_{q}\***ta})[0,{u\***ta}]

log(q)log(euq​θ)[0,uθ]接下来是单位四元数的幂运算

q^{t}(e^{u_{q}\***ta})^{t}e^{u_{q}(t\***ta)}[cos(tθ),

sin(tθ)u]

qt(euq​θ)teuq​(tθ)[cos(tθ),sin(tθ)u]可以看到一个单位四元数的

t结合。

这些运算会在之后讨论四元数插值时非常有用。

限于篇幅四元数插值的讲解见下一篇博客《三维空间刚体运动4-2四元数插值lerpNlerpSlerpSquad等》

5.4

上面给出四元数的指数形式后可推导出一个重要的指函数性质这一性质下一篇将用到。

首先看推论

四元数推论1

\mathbb{H}_{1},p[a,b\mathbf{v}]

q∈H1​,p[a,bv]其中

\mathbb{R},\mathbf{v}\in\mathbb{R}^{3}

a,b,r∈R,v∈R3如果

q[r,\mathbf{v}]q^{*}[r,\mathbf{v^{}}]

q[r,v]q∗[r,v′]那么

q[a,b\mathbf{v}]q^{*}[a,b\mathbf{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

\mathbb{H}_{1},p[\cos\***ta,\sin\***ta\mathbf{v}],t\in\mathbb{R}

定理证明

q[\cos\***ta,\sin\***ta\mathbf{v}]q^{*}[\cos\***ta,\sin\***ta\mathbf{v^{}}]

q[cosθ,sinθv]q∗[cosθ,sinθv′]因此得到

exp

q(\exp(t[0,\***ta\mathbf{v}]))q^{*}

q(\exp[0,t\***ta\mathbf{v}])q^{*}

q([\cos

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​

现在我们通过两个小程序实际演练四元数的运算。

其中四元数的基础运算和高阶运算程序实现放在下一讲Slerp中。

6.1

第一个小程序是演示四元数的常规运算包括与旋转矩阵、旋转向量的转换以及用四元数旋转一个向量如下所示

#includeiostream

#includeeigen3/Eigen/Geometryusing

namespace

**argv){//Eigen/Geometry模块提供了各种旋转和平移的表示3D旋转矩阵直接使用Matrix3d或Matrix3fMatrix3d

rotation_matrix

Matrix3d::Identity();//旋转向量使用AngleAxis它底层不直接是Matrix但运算可以当做矩阵因为重载了运算符AngleAxisd

Vector3d(0,

1));//设置输出精度cout.precision(3);coutrotation

matrix

\nrotation_matrixendl;coutrotation

vector

\nrotation_vector.matrix()endl;//旋转向量转换的矩阵可以直接赋值给旋转矩阵rotation_matrix

rotation_vector.toRotationMatrix();coutrotation

vector

\nrotation_matrixendl;//旋转向量Vector3d

v(1,

v.transpose()endl;//四元数可以直接把AngleAxis赋值给四元数反之亦然Quaterniond

Quaterniond(rotation_vector);//coeffs:多项式系数coefficients)其顺序为(x,y,z,w)w为实部xyz为虚部coutquaternion

from

q.coeffs().transpose()endl;//也可以直接把旋转矩阵赋给它q

Quaterniond(rotation_matrix);coutquaternion

from

q.coeffs().transpose()endl;//使用四元数旋转一个向量使用重载的乘法即可//注意q*v在数学上是qvq^{-1}v_rotated

q*v;cout(1,0,0)

v_rotated.transpose()endl;//用常规向量乘法表示qvq^{-1}则计算如下Quaterniond

q_rotate_v

q*Quaterniond(0,1,0,0)*q.inverse();coutshould

equal

q_rotate_v.coeffs().transpose()endl;return

0;输出结果如下

0请读者注意程序代码通常和数学表示有一些细微的差别。

例如通过运算符重载四元数和三维向量可以直接计算乘法但在数学上则需要先把向量转成虚四元数再利用四元数乘法进行计算同样的情况也适用于变换矩阵乘三维向量的情况。

总体而言程序中的用法会比数学公式更灵活。

6.2

例子设有小萝卜一号和小萝卜二号位于世界坐标系中。

记世界坐标系为

R_{1}

TRk​,W​,k1,2也就是世界坐标系到相机坐标系的变换关系。

现在小萝卜一号看到某个点在自身的坐标系下坐标为

0.5

pR1​​[0.5,0,0.2]T求该向量在小萝卜二号坐标系下的坐标。

这是一个非常简单但又具有代表性的例子。

在实际场景中你经常需要在同一个机器人的不同部分或者不同机器人之间转换坐标。

计算过程也很简单只需计算

p_{R_{2}}T_{R_{2},W}T_{W,R_{1}}p_{R_{1}}

#includeiostream

#includeeigen3/Eigen/Geometryusing

namespace

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.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

Twr1.inverse()*pr1;coutpr2.transposepr2.transpose()endl;return

}输出结果如下

本文基于《视觉SLAM十四讲从理论到实践》和《Quaternions,

Interpolation

Animation》编写但相对于原文会适当精简同时为便于全面理解会收集其他网络好文根据作者理解加入一些注解和扩展知识点如果您觉得还不错请一键四连点赞关注收藏评论让更多的人看到。

参考文献

《视觉SLAM十四讲从理论到实践》高翔、张涛等著中国工信出版社Quaternions,

Interpolation

Animation四元数与三维旋转四元数与欧拉角RPY角的相互转换如何形象地理解四元数