matlab 罗德里格公式,旋转矩阵，四元素，欧拉角

旋转变换

旋转变换最为直观的表示方法是“轴-角”：绕着某一个过原点轴，旋转某一角度。

轴可以用一个单位长度的点[w1,w2,w3][w1,w2,w3]表示：原点到该点的射线即为此轴。

使用右手座标系，拇指指向轴方向，四指方向即为旋转的方向。

一个旋转变换可以用用欧拉角、四元数或者旋转矩阵表示。以下讨论不同表示方法之间的关系，以及旋转变换的合成、取逆等操作。

旋转矩阵

旋转可以看做一种特殊的座标变换，而座标变换可以用用3×33×3矩阵RR来表示。对一个座标施加旋转的结果是x′=Rxx′=Rx。

旋转矩阵可以在不同座标系之间进行变换，但不能进行“反演”，即不能在左手系和右手系之间进行变换。

旋转矩阵是正交矩阵，即|R|=1|R|=1，旋转变换不改变向量的长度。

欧拉角的物理意义

任何一个旋转可以表示为依次绕着三个旋转轴旋三个角度的组合。这三个角度称为欧拉角。

三个轴可以指固定的世界座标系轴，也可以指被旋转的物体座标系的轴。三个旋转轴次序不同，会导致结果不同。

本文中提到的欧拉角指：绕着世界座标系的x,y,z轴，依次旋转的结果。其取值范围如下：

θx∈(−π,π),θy∈(−π2,π2),θz∈(−π,π)

欧拉角→→旋转矩阵

单独绕一个轴旋转θθ角度的旋转矩阵为：

如果依次绕x轴、y轴、z轴旋转，该变换的旋转矩阵为：

R=Rz⋅Ry⋅Rx

记三个轴欧拉角的正弦和余弦函数为sx,cx,sy,cy,sz,czsx,cx,sy,cy,sz,cz。使用matlab的syms功能可以轻松推导旋转矩阵RR：

⎡⎣⎢cyczcysz−syczsxsy−cxszcxcz+sxsyszcysxsxsz+cxczsycxsysz−czsxcxcy⎤⎦⎥

[cyczczsxsy−cxszsxsz+cxczsycyszcxcz+sxsyszcxsysz−czsx−sycysxcxcy]

旋转矩阵→→欧拉角

设旋转矩阵i行j列元素为rijrij。根据旋转矩阵的表达式，利用三角函数可以推导出欧拉角取值：

θx=atan2(r32,r33)

θy=atan2(−r31,r232+r233−−−−−−−√)

θy=atan2(−r31,r322+r332)

θz=atan2(r21,r11)

四元数的物理意义

设有一个通过原点[0,0,0][0,0,0]的旋转轴，该轴上单位长度的点为[w1,w2,w3][w1,w2,w3]。绕此轴旋转θθ角的变换可以用一个向量表示：

[cosθ2,w1sinθ2,w2sinθ2,w3sinθ2]

[cos⁡θ2,w1sin⁡θ2,w2sin⁡θ2,w3sin⁡θ2]

也记为q=[q0,q1,q2,q3]q=[q0,q1,q2,q3]，或者q=q0+q1i+q2j+q3kq=q0+q1i+q2j+q3k。四元数的模长为1：

q20+q21+q22+q23=cos2θ2+sin2θ2(w21+w22+w33)=cos2θ2+sin2θ2=1

q02+q12+q22+q32=cos2⁡θ2+sin2⁡θ2(w12+w22+w33)=cos2⁡θ2+sin2⁡θ2=1

四元数→→旋转矩阵

此段公式暂不确定，请您参看评论讨论

利用罗德里格旋转公式可以获得绕某过原点一轴[w1,w2,w3][w1,w2,w3]旋转某一角度θθ的旋转矩阵RR：

⎡⎣⎢cosθ+w21(1−cosθ)w1w2(1−cosθ)−w2sinθw1w2(1−cosθ)cosθ+w22(1−cosθ)w1sinθw2sinθ−w1sinθcosθ⎤⎦⎥

[cos⁡θ+w12(1−cos⁡θ)w1w2(1−cos⁡θ)w2sin⁡θw1w2(1−cos⁡θ)cos⁡θ+w22(1−cos⁡θ)−w1sin⁡θ−w2sin⁡θw1sin⁡θcos⁡θ]

代入四元组的表示方法，可得：

⎡⎣⎢⎢1−2q22−2q232q1q2+2q3q02q1q3−2q2q02q1q2−2q3q01−2q21−2q232q2q3+2q1q02q1q3+2q2q02q2q3−2q1q01−2q21−2q22⎤⎦⎥⎥

[1−2q22−2q322q1q2−2q3q02q1q3+2q2q02q1q2+2q3q01−2q12−2q322q2q3−2q1q02q1q3−2q2q02q2q3+2q1q01−2q12−2q22]

旋转矩阵→→四元数

此段公式暂不确定，请您参看评论讨论

根据旋转矩阵的表达式，利用三角函数性质，可以由旋转矩阵得到四元数：

q0=1+r11+r22+r33−−−−−−−−−−−−−−−√2

q0=1+r11+r22+r332

q1=r32−r234q0

q2=r13−r314q0

q3=r21−r124q0

开根号要求1+Tr(R)>01+Tr(R)>0(其中TrTr表示矩阵的迹，等于对角元素之和，也等于特征值之和)，分式要求q0≠0q0≠0。

某些情况下(例如θ=πθ=π)，q0q0接近零，Tr(R)Tr(R)接近-1，需要使用以下方式求解(来源于此处)。

如果r11,r22,r33r11,r22,r33中，r11r11最大：

S=1+r11−r22−r33−−−−−−−−−−−−−−−√

S=1+r11−r22−r33

q0=r32−r23S

q1=S/4

q2=r12+r21S

q3=r21+r12S

如果r11,r22,r33r11,r22,r33中，r22r22最大：

S=1−r11+r22−r33−−−−−−−−−−−−−−−√

S=1−r11+r22−r33

q0=r13−r31S

q1=r12+r21S

q2=S/4

q3=r23+r32S

如果r11,r22,r33r11,r22,r33中，r33r33最大：

S=1−r11−r22+r33−−−−−−−−−−−−−−−√

S=1−r11−r22+r33

q0=r21−r12S

q1=r13+r31S

q2=r23−r32S

q3=S/4

变换的逆

使用欧拉角表示时，必须颠倒三个旋转轴的顺序，同时对旋转角度取反。

使用旋转矩阵表示时，求矩阵的逆即可：R∗=R−1R∗=R−1。

使用四元组表示时，考虑其物理意义，对后三位取反即可：q−1=[q0,−q1,−q2,−q3]q−1=[q0,−q1,−q2,−q3]。

向量的叉乘

接下去讨论之前，需要先复习向量的叉乘。

三维空间中的一个点可以表示为向量[xi,yj,zk][xi,yj,zk]，其中i,j,ki,j,k是三个座标轴：[1,0,0],[0,1,0],[0,0,1][1,0,0],[0,1,0],[0,0,1]。

两个向量a,ba,b叉乘的结果是一个向量，其长度为|a||b|sinθ|a||b|sin⁡θ。θθ表示从向量aa到bb的小于180°的角度。其方向垂直于a,ba,b所在平面，遵循右手定则：四指从aa转向bb，拇指方向为叉乘结果方向。

四元组作为一种向量，其叉乘涉及到虚数单位的乘法，遵循以下原则：

i×i=−1

i×j=−j×i

i×j=k,j×k=i,k×i=j

叉乘满足反交换律：

a×b=−b×a

联想叉乘的物理意义：交换了a,ba,b顺序，则拇指方向反转。

叉乘满足加法分配律：

(a+b)×c=a×c+b×c

特别要注意，叉乘不满足结合律：

(a×b)×c≠a×(b×c)

四元数的叉乘

四元数既不是矢量也不是标量。可以看做一个标量q0q0和一个三维矢量[q1,q2,q3][q1,q2,q3]的结合体：q=q0+q1i+q2j+q3kq=q0+q1i+q2j+q3k。

两个四元数a1+a2i+a3j+a4ka1+a2i+a3j+a4k，b1+b2i+b3j+b4kb1+b2i+b3j+b4k，相乘的结果q=a×bq=a×b：

q0=a1b1−a2b2−a3b3−a4b4

q1=a1b2+a2b1+a3b4−a4b3

q2=a1b3−a2b4+a3b1+a4b2

q3=a1b4+a2b3−a3b2+a4b1

四元数叉乘不满足交换律、结合律。

变换的组合

使用欧拉角表示时，很难直接组合两个变换。

使用旋转矩阵表示时，依次做矩阵相乘即可：R=R2R1R=R2R1。

使用四元组表示时，需要对两个四元组叉乘：q=q2×q1q=q2×q1。

注意，先发生的变换要放在乘号右侧。

由于叉乘不满足结合律，当有一系列变化q1,q2,q3…q1,q2,q3…陆续发生时，要用括号保证乘法发生的顺序：

q=q4×(q3×(q2×q1))

使用四元数

首先把待旋转的点(x,y,z)(x,y,z)表示成四元数形式：p=[0,x,y,z]p=[0,x,y,z]。这里的四元数不再表示一个旋转，所以模长不一定为1。

对该点pp施加旋转变换qq，通过四元组的两次叉乘实现：p′=q×p×q−1=[0,x′,y′,z′]p′=q×p×q−1=[0,x′,y′,z′]。

利用matlab的syms功能(参看附录)，可以得到对三维点[x,y,z][x,y,z]施加变换[q0,q1,q2,q3][q0,q1,q2,q3]的结果：

x′=x(q20+q21−q22−q23)+2y(q1q2−q0q3)+2z(q0q2+q1q3)

x′=x(q02+q12−q22−q32)+2y(q1q2−q0q3)+2z(q0q2+q1q3)

y′=2x(q0q3+q1q2)+y(q20−q21+q22−q23)+2z(−q0q1+q2q3)

y′=2x(q0q3+q1q2)+y(q02−q12+q22−q32)+2z(−q0q1+q2q3)

z′=2x(−q0q2+q1q3)+2y(q0q1+q2q3)+z(q20−q21−q22+q23)

———————

作者：shenxiaolu1984

来源：CSDN

原文：https://blog.csdn.net/shenxiaolu1984/article/details/50639298