)
方向余弦矩阵
基本概念
方向余弦:
在坐标系R中设v为一个空间向量,在R坐标系下的投影为(vx,vy,vz)
与其x y z 轴分别成
αβγ角度,则
cosα,cosβ,cosγ分别为v在三轴的方向余弦,大小分别等于|vx|,|vy|,|vz|。
方向余弦矩阵(direction cosine matrix)
在一个平面内对向量进行旋转相当于对坐标进行旋转,初始状态下令载体坐标系和参考坐标系完全重合。
方向余弦矩阵便是存放着一系列方向余弦,设r(x,y,z)为参考坐标系,O(X1,Y1,Z1)为载体坐标系,载体转动时,载体坐标系会相对于参考坐标系转动,把载体坐标系的三个轴当做三个单位向量(vx,vy,vz),每个载体坐标轴都可以在参考坐标系上找到三个对应的方向余弦。最终得到九个方向余弦,把九个方向余弦写成矩阵形式就是方向余弦矩阵。
而方向余弦矩阵的写法有两种,一种是载体坐标系相对于参考坐标系,一种是参考坐标系相对于载体坐标系。
C0r=⎣⎡c11c12c13c21c22c23c31c32c33⎦⎤或Cr0=⎣⎡c11c21c31c12c22c32c13c23c33⎦⎤
C_0^r表称O系对于r系的方向余弦矩阵,这两个矩阵互为转置矩阵。
C12=cos(xr.x0)载体坐标系上的$x_0相对于参考坐标系上的x_r的方向余弦,
C12=cos(xr,y0)同理
y0相对于参考坐标系上xr的方向余弦
已知欧拉角求方向余弦
ψθγ
⎣⎡cosγcosψ+sinγsinψsinθsinψcosθsinγcosψ−cosγsinψsinθ−cosγsinψsinθcosψcosθ−sinγsinψ−cosγcosψsinθ−sinγcosθsinθcosγcosθ⎦⎤
已知方向余弦求欧拉角
方向余弦矩阵:
⎣⎡T11T12T13T21T22T23T31T32T33⎦⎤
推出:
横滚角:
θ=arcsinT32
俯仰角:
γ=arctan(−T33T32)
偏航角:
ψ=arctan(T22T12)
四元数
基本概念
四元数是由实数和虚数组成的一组超复数,假设Q是一个四元数。
Q = w + xi + yj + zj
其中w表示四元数的实数大小,x,y,z表示虚数大小,实部单位为1,虚部单为为i,j,k。
四元数一般表示:
q
=[wv
]=[w(xyz)]
四元数表征姿态
其中w为实数部分,
v
为矢量部分,首先讨论如何把一个空间向量转换到四元数。
假设在三维空间中有一个点(x,y,z),拓展到四元数空间有
p
=[0(xyz)]
p
就是三维空间中一点四元数的表示,而旋转是通过旋转四元数来实现的。现在假设空间中有一个向量
n
,
(nx,ny,nz),若用四元数来表示点p(x,y,z)绕向量
n
旋转
θ角度后
p,
定义旋转四元数为
p
则对于本次旋转有:
p
=[cos
(2θ) sin
(2θ)
n
]=[cos
(2θ)
. sin
(2θ)
nx
. sin
(2θ)
ny
. sin
(2θ)
nz
]
(其中.为分隔用)
其中
nx
|
ny
|
nz
|
nx
=
nx*i |
ny
=
ny*j |
nz
=
nz*k |
若想得到旋转后的矩阵只需要执行以下式子
p,
=q
p
q
−1
(q
−1为q
的逆矩阵)
想要推导下面的公式需要了解两个概念
<1四元数的共轭
四元数的共轭就是让四元数的向量部分取负,记作
p
=[w
v
]=[w -
v
]=[w (-x -y -z)]
四元数与他的共轭代表反向的角位移,因为相当于旋转轴反向
<2四元数的逆
四元数的逆就是他的共轭除以他的模
q−1
=(∣q
∣)(q
∗)
一般使用单位四元数,所以他的逆和共轭是相等的
因为公式中涉及到四元数的乘法,在表示旋转时用叉乘,计算公式如下
[w1(x1y1z1)][w2(x2y2z2)]=⎣⎢⎢⎡w1w2−x1x2−y1y2−z1z2⎝⎛w1x2+x1w2+z1y2−y1z2w1y2+y1w2+x1z2−z1x2w1z2+z1w2+y1x2−x1y2⎠⎞⎦⎥⎥⎤
example:
假设空间中有一个点p(0,1,0)饶z轴逆时针旋转90度,求旋转后的
p,点。理论上
p,点为(-1,0,0)。
首先把p(0,1,0)拓展成一个四元数
p
=(0,0,j,0),然后定义旋转四元数,旋转轴为z轴化为单位向量(0,0,1),因为旋转角度为90度所以------
q
=(cos45。,0,0,1∗sin45。k)=(22
,0,0,22
k)
单位四元数的逆的共轭是相等的
q
−1=(22
,0,0,−22
k)
由四元数叉乘计算公式
p,
=q
p
q
−1 用上述乘法公式计算
q
p
=(0,22
kj,22
j,0)
k*j=-i再乘以
q
−1得到p
,=[0,−i,0,0],带回去得到点[-1,0,0]即为旋转之后的坐标。
四元数与方向余弦
由前面的介绍知道了旋转四元数
q=cos2θ+x∗sin2θ∗i+y∗sin2θ∗j+z∗sin2θ∗k
这里令
q0=cos2θ,q1=sin2θ,q2=sin2θ,q3=sin2θ,
q=
q0+q1i+q2j+q3k四元数旋转的方向余弦矩阵公式如下:
⎣⎡xbybzb⎦⎤=⎣⎡q02+q12−q22−q322(q1q2−q0q3)2(q1q2−q0q3)2(q1q2+q0q3)q02−q12+q22−q322(q2q3−q0q1)2(q1q3−q0q2)2(q0q1+q2q3)q02−q12−q22+q32⎦⎤⋅⎣⎡xnynzn⎦⎤
四元数表示的旋转矩阵公式
姿态表示的三种方法
欧拉角
直观的欧拉角 横滚角roll 俯仰角pitch 偏航角 yaw
方向余弦矩阵
⎣⎡cosγcosψ+sinγsinψsinθsinψcosθsinγcosψ−cosγsinψsinθ−cosγsinψsinθcosψcosθ−sinγsinψ−cosγcosψsinθ−sinγcosθsinθcosγcosθ⎦⎤
四元数
⎣⎡xbybzb⎦⎤=⎣⎡q02+q12−q22−q322(q1q2−q0q3)2(q1q2−q0q3)2(q1q2+q0q3)q02−q12+q22−q322(q2q3−q0q1)2(q1q3−q0q2)2(q0q1+q2q3)q02−q12−q22+q32⎦⎤⋅⎣⎡xnynzn⎦⎤
spark−1