-
四元数转化成欧拉角
笔者：奋斗
修改时间：
2015.11.23
一．
姿态解算（以匿名版程序为例）
首先
，程序中一般用了两种求解姿态的方法，一种为欧拉角法，一种为四元
数法。由四元数法
将
MPU6050
的值融合成四元，再由欧拉角法把四元转换成
欧
拉角送入串级
PID
控制器。
（
1
）欧拉角法静止状态，或者总加速度只是稍微大于
g
时，由加计算出的值比
较准确。
使用欧拉角表示姿态，
令
Φ
,
θ
和
Φ
代表
ZYX
欧拉角，
分别称为偏航角、
俯仰角和横滚角。
载体坐标系下的
加
速
度
(
a
x
B,a
y
B
,
a
z
B
)
和参考坐标系下的加
速度
(a
x
N,
a
y
N,
a
z
N)
之间的关系可表示为
(1)
。
其中
c
和
s
分别代表
cos
和
sin
。
axB,ayB,azB
就是
mpu<
/p>
读出来的三个值。
这个矩阵就是三个旋
转矩阵相乘得到的，因为矩阵的乘法可以表示旋转。
c
?
c
?
?
axB
?
?
?
ayB
?
?
?
?
c
?
s
?
?
s
?
s
?
c
?
?
?
?
?
?
azB
?
?
?
?
s
?
s
?
?
c
?
< br>s
?
c
?
?
a
xN
?
?
0
?
?
?<
/p>
?
?
?
a
yN
?
?
?
0
?
（
2
）
?
a
< br>?
?
?
?
zN
?
?
g
?
c
?
s
?<
/p>
c
?
c
?
?
s
?
s
?
s
?
?
s
?
c
?
?
c
?
s
?
s
?
?
s<
/p>
?
?
?
axN<
/p>
?
?
ayN
?<
/p>
(1)
s
?
c
?
?
?
?
?
c
?
c
?
?
?
?
?
azN
?
?
飞
行
器
< br>处
于
静
止
状
态
，
此
时
参
考
系
下
的
加
速
度
等
于
重
力
加
速
度
，
< br>即
把（
2
）代入（
1
）可以解得
?
?
arctg
(
a
xB
a
?
a
2
yB
2
zB
)
（
3
）
?
?
arctg
?
?
a
yB
?
?
（
4
）
a
?
< br>zB
?
即为初始俯仰角和横滚角，
通过加速度计得到载体坐标系下的加速度即可将
其解出，偏航角可以通过电子罗盘求出
。
（
2
）
四元数法<
/p>
（通过处理单位采样时间内的角增量
(mpu
的陀螺仪得到的就是角
增量
)
，为
了避免噪声的微分放大，应该直接用角增量）
void
IMUupdate(float gx, float gy, float gz, float ax,
float ay, float az)
{
float norm;
//
float hx, hy, hz, bx, bz;
float vx, vy, vz;
// wx, wy, wz;
float ex, ey, ez;
//
先把这些用得到的值算好
float q0q0 = q0*q0;
float q0q1 =
q0*q1;
float
q0q2 = q0*q2;
// float q0q3 = q0*q3;
float q1q1 =
q1*q1;
// float q1q2 = q1*q2;
float q1q3 =
q1*q3;
float
q2q2 = q2*q2;
float q2q3 = q2*q3;
float q3q3 = q3*q3;
if(ax*ay*az==0)
return;
norm = sqrt(ax*ax + ay*ay + az*az);
//acc
数据归一化
ax = ax /norm;
ay = ay / norm;
az = az / norm;
//
estimated direction of gravity and flux (v and w)
vx = 2*(q1q3 - q0q2);
//
四元素中
xyz
vy = 2*(q0q1 + q2q3);
vz = q0q0 -
q1q1 - q2q2 + q3q3
// error is sum of cross
product between reference direction of fields and
direction
measured by sensors
ex = (ay*vz -
az*vy)
//
向量外积在相减得到差分就是误差
ey = (az*vx -
ax*vz)
ez =
(ax*vy - ay*vx)
exInt = exInt + ex * Ki;
//
对误差进行积分
eyInt = eyInt +
ey * Ki;
ezInt
= ezInt + ez * Ki;
// adjusted
gyroscope measurements
gx = gx + Kp*ex + exInt;
//<
/p>
将误差
PI
后补偿到陀螺仪，即补偿零点
漂移
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt;
//
这里的
gz
由于没有观测者进行矫正会产生漂移，
表
< br>现出来的就是积分自增或自减
// integrate quaternion
rate and normalise
//
四元素的微分方程
q0 = q0 +
(-q1*gx - q2*gy - q3*gz)*halfT;
q1 = q1 + (q0*gx + q2*gz -
q3*gy)*halfT;
q2 = q2 + (q0*gy - q1*gz +
q3*gx)*halfT;
q3 = q3 + (q0*gz + q1*gy -
q2*gx)*halfT;
// normalise quaternion
norm = sqrt(q0*q0 + q1*q1 +
q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm;
//Q_ = atan2(2 * q1 * q2 + 2 * q0 * q3,
-2 * q2*q2 - 2 * q3* q3 + 1)*
57.3; //
yaw
Q_ANGLE.Y
= asin(-2 * q1 * q3 + 2 *
q0* q2)* 57.3;
// pitch
Q_ANGLE.X = atan2(2 * q2 * q3 + 2 * q0
* q1, -2 * q1 * q1 - 2 * q2* q2 + 1)*
57.3; // roll
}
下
面对上面的程序逐条解释。姿态矩阵可以由以下两种方式表示。第一个就
是上图所说的欧
拉角法（式（
1
）
）
< br>，还有一个就是四元数法
2(q1q1
?
q
0q
3)
2(q1q
3
?
q
0q
2)
?
q
0
^
2
?
q
1^
2
?
< br>q
2
^
2
?
q
3
^
2
?
?
CbR
?
?
2(q1q
2
?
q
0q
3)
1
?
2(q1^
< br>2
?
q
3
^
2)
2(q
2q
< br>3
?
q
0q1)
?
?
?
2(q1q
3
?
q
0q
2)
2(q
2q
3
?
q
0q1)
q
0
^
2
?
q
1^
2
?
q
2
^
2
< br>?
q
3
^
2
?
?
?
注意：这里是
C
b
R,
假设
b
为四旋翼固连坐标系，
R
为参考坐标系，那么
CbR
表
示
b
系到
R
系的坐标变换矩阵，由于
(1)
式表示的为
R
系到
b
系的坐标矩阵，要
用上式表示，则要对四元数法矩阵求逆，又因为该矩阵
为正交阵，逆等于转置，
则描述
R
系到
b
系的四元数矩阵为
2(q1q1
?
q
0q
3)
2(q1q
3
?
q
0q
2)
?
q
0
^
2
?
q
1^
2
?
q
2
^
2
?
q
3
^
2
?
?
CRb
?
?
2(q1q
2
?
q
0q
3)
1
?
2(q1^
2
?
q
3
^
2)
2(q
2q
3
?
q
0q1)
?
?
?
2(q1q
3
?
q
0q<
/p>
2)
2(q
2q
3
?
q
0q1)
q
0
^
2
?
q
1^
2
?<
/p>
q
2
^
2
?
q
3
^
2
?
?
?
此时矩阵跟
1
式矩阵一一对应。
-
-
-
-
-
-
-
-
-
上一篇：PhoenixRC
下一篇：常用的文件格式有哪些