四元数、欧拉角和旋转矩阵
四元数、欧拉角和旋转矩阵都可以描述三维空间中的旋转,三者可以相互转化。这里四元数指的都是单位四元数,不改变向量的模,和旋转矩阵是正交的一个道理。欧拉角的转动顺序为 Z→Y→X ,转动的角度分别为 (ψ,θ,ϕ) 。
欧拉角介绍
欧拉角包括偏航角(yaw) ψ ,俯仰角(pitch) θ ,滚转角(roll) ϕ ,导航坐标系 S−Oxgygzg 经过三次旋转可以得到机体坐标系 B−xbybzb 。需要注意的是,这里旋转的是坐标轴,不是向量。旋转过程遵循右手定则,过程如下:
- 以
zg
正方向为轴,
S
系右旋
ψ 得到过渡坐标系 S′−Ox′y′z′ - 以 y′ 正方向为轴, S′ 系右旋 θ 得到过渡坐标系 S′′−Ox′′y′′z′′
- 以 x′′ 正方向为轴, S′′ 系右旋 ϕ 得到机体坐标系 B−Oxbybzb
每次旋转都用一个旋转矩阵表示如下:
第一次旋转:
第二次旋转:
第三次旋转:
将三个旋转矩阵相乘得到:
Rbn 就是用欧拉角表示的旋转矩阵。
四元数介绍
四元数一般形式为:
q⃗ =w+xi⃗ +yj⃗ +zk⃗
,如果四元数的模为1,称之为单位四元数,可以表示为:
q⃗ =cos(θ2)+sin(θ2)(cos(ϕx)i⃗ +cos(ϕy)j⃗ +cos(ϕz)k⃗
,含义:以当前坐标系中
(cos(ϕx),cos(ϕy),cos(ϕz))
为轴,旋转
θ
角度。下面以导航坐标系(东北天)到机身坐标系的转化为例,介绍四元数的用法。
四元数的旋转是通过乘法实现的:
p′=qpq−1
。
q−1
表示四元数的逆,对于单位四元数,
q−1=q∗
。(注:四元数的乘法和共轭去网上查)
比如:取
p=[1,1,1]
,
q=[cos(90∘2),0,0,sin(90∘2)]=[0.707,0,0,0.707]
,在MATLAB中计算:
>> q=[0.707,0,0,0.707];
>> q1=quatmultiply(q,[0,1,1,1]);
>> quatmultiply(q1,quatinv(q))
ans =
0 -1.0000 1.0000 1.0000
旋转之后的向量为 (−1,1,1) ,向量和坐标轴的转动是相对的,因此有两种理解方式:
- 坐标轴固定不动,向量以Z正方向为轴,按照右手定则旋转了 45∘ ,计算得到的是新向量在此坐标系下的坐标。
- 向量固定不动,坐标轴以Z正方向为轴,按照左手定则旋转了 45∘ ,计算得到的是原向量在新的坐标系下的坐标。
理解两种情况下转动的对象和转动的正方向是很重要的,否则在进行三者之间的相互转换时会出错。
若记
q=(q0,q1,q2,q3)
,
p=(x,y,z)
,
p′=(x′,y′z′)
,则
p′=qpq−1
也可以写成矩阵形式:
此矩阵便是四元数对应的旋转矩阵。
下面我们用四元数表示欧拉角的三次旋转,并推导出二者的转化关系。(注:根据四元数旋转的第二种理解,如果我们旋转的是坐标轴,那么应该按照左手定则指定旋转的正方向,而欧拉角旋转时采用的是右手定则,因此四元数转动角度为-
θ
)
也许你会发现这与 WIKI上的结果是互逆的或者互为共轭。原因很简单:按照我们对欧拉角的定义,WIKI上的Body坐标系就相当于我们的导航坐标系,Lab坐标系相当于我们的机体轴系。如果 q⃗ 可以实现 B→S ,那么 q⃗ ∗ 必然可以实现 S→B 。
相互转换
欧拉角转换为四元数
根据(3)式,可以实现欧拉角转换四元数。C++实现代码如下:
void cEulerAngle::toQuat(cQuaternion &quat) const
{
double cos_hroll = cos(_roll / 2.0);
double sin_hroll = sin(_roll / 2.0);
double cos_hpitch = cos(_pitch / 2.0);
double sin_hpitch = sin(_pitch / 2.0);
double cos_hyaw = cos(_yaw / 2.0);
double sin_hyaw = sin(_yaw / 2.0);
quat._w = cos_hroll*cos_hpitch*cos_hyaw + sin_hroll*sin_hpitch*sin_hyaw;
quat._x = cos_hroll*sin_hpitch*sin_hyaw - sin_hroll*cos_hpitch*cos_hyaw;
quat._y = -sin_hroll*cos_hpitch*sin_hyaw - cos_hroll*sin_hpitch*cos_hyaw;
quat._z = -cos_hroll*cos_hpitch*sin_hyaw + sin_hroll*sin_hpitch*cos_hyaw;
}
四元数转化为欧拉角
(1)式和(2)式两个旋转矩阵相等,可以得到:
然后呢,
atan()
函数的返回值位于
(−π2,π2)
之间,而
ψ
和
ϕ
的范围为
(−π,π)
,因此应该换用
atan2()
函数。同时,
asin()
的返回值位于
(−π2,π2)
,这也就是俯仰角的范围。不要怀疑俯仰角的范围是不是小了,因为这三个角的范围已经能够完全表述三维空间中的旋转了。最终得到:
C++示例代码如下:
void cQuaternion::toEuler(cEulerAngle &angle) const
{
angle._roll = atan2(2 * (_z*_y - _w*_x), _w*_w - _x*_x - _y*_y + _z*_z);
angle._pitch = asin(-2 * (_w*_y + _z*_x));
angle._yaw = atan2(2 * (_y*_x - _w*_z), _w*_w + _x*_x - _y*_y - _z*_z);
}
旋转矩阵转四元数
根据(2)式,便可以推得:
void cDcm::toQuat(cQuaternion &quat) const
{
double trace = m11 + m22 + m33;
double temp = 0.0;
if (trace > 0)
{
temp = sqrt(trace + 1.0)*0.5;
quat._w = temp;
temp = temp*0.5;
quat._x = (m23 - m32)*temp;
quat._y = (m31 - m13)*temp;
quat._z = (m12 - m21)*temp;
}
else
{
if (m11 > m22 && m11 > m33)
{
temp = 2.0 * sqrt(1.0 + m11 - m22 - m33);
quat._w = (m32 - m23) / temp;
quat._x = 0.25* temp;
quat._y = (m12 + m21) / temp;
quat._z = (m13 + m31) / temp;
}
else if (m22 > m33)
{
temp = 2.0 * sqrt(1.0 + m22 - m11 - m33);
quat._w = (m13 - m31) / temp;
quat._x = (m12 + m21) / temp;
quat._y = 0.25 * temp;
quat._z = (m23 + m32) / temp;
}
else
{
temp = 2.0 * sqrt(1.0 + m33 - m11 - m22);
quat._w = (m21 - m12) / temp;
quat._x = (m13 + m31) / temp;
quat._y = (m23 + m32) / temp;
quat._z = 0.25 * temp;
}
}
}
旋转矩阵与欧拉角相互转换
欧拉角到旋转矩阵根据(1)式便可以得到,旋转矩阵到欧拉角也是求反正弦、反余弦,代码比较简单,不贴了。
2164



被折叠的 条评论
为什么被折叠?



