基于六轴陀螺仪加速度传感器的姿态解算方法
基于六轴陀螺仪加速度传感器的姿态解算方法
本文将详细介绍基于六轴陀螺仪加速度传感器的姿态解算方法。通过角速度和加速度数据，结合四元数和互补滤波算法，实现对无人机等设备的姿态估计。
1. 六轴陀螺仪加速度传感器
六轴陀螺仪加速度传感器包含3轴陀螺仪（角速度传感器）和3轴重力加速度传感器。这种传感器常用于无人机/四旋翼的姿态解算。常见的六轴传感器型号有MPU6050和ICM-20602。
2. 姿态解算
姿态解算的目的是根据六轴传感器的数据计算出当前物体的姿态角，即欧拉角。
3. 基本概念
1) 地面惯性坐标系
地面惯性坐标系用于描述飞行器相对于地面的运动状态。通常Z轴垂直向下，坐标系按照右手系建立。
2) 机体坐标系
机体坐标系的原点位于飞行器质心，OX轴平行于机身轴线或对称面机翼弦线指向机头方向，OY轴垂直于对称面指向右翼，OZ轴在飞机对称面内，垂直于OX轴指向下方。坐标系符合右手系建立规定。
3) 欧拉角
机体坐标系与地面惯性坐标系之间的夹角就是飞行器的姿态角，又称为欧拉角。
- 俯仰角（pitch）：机体轴与地平面（水平面）之间的夹角，机体抬头为正方向。
- 偏航角（yaw）：机体轴在水平面上投影和地轴xe之间的夹角，以机头右偏为正方向。
- 滚转角（roll）：机体对称面绕机体轴转过的角度，向右滚为正方向。
通过这3个值就可以唯一确定飞行器的姿态。
4. 由角速度求欧拉角
这里采用四元数法由角速度求解欧拉角，共分2个步骤：先由角速度求四元数，再由四元数求欧拉角。
1) 四元数
引入四元数是为了方便通过角速度获取欧拉角。将角速度转换为四元数是其中的过渡过程。这里，采用一阶龙格库塔法求四元数。推导过程略，这里直接给出结论。
其中，各q初始值，分别为机体坐标系下X轴，Y轴，Z轴角速度，T为采样周期。
2) 由四元数求欧拉角
前面我们由角速度求得四元数，根据四元数我们可以求得欧拉角。推导过程略，直接给出结论。
其中，为前面由角速度算得的四元数，r(roll),p(pitch),y(yaw)分别为滚转角，俯仰角，偏航角。
至此，我们通过角速度求得欧拉角。
5. 由加速度求欧拉角
由加速度求欧拉角，原理上是根据各轴加速度（重力加速度在各轴的投影）来求解各个角度，这里可以通过绕各个轴旋转（按ZYX方式）求出各个轴加速度，再进一步求取各个角度，公式推导如下：
由于绕Z轴旋转时，感受到的加速度（重力加速度分量）是不变的，因此，由加速度无法计算偏航角（yaw）。通过对上式求解可得：
其中，分别为X轴，Y轴，Z轴加速度（重力加速度分量），r,p分别为滚转角，俯仰角。
至此，我们通过加速度求得欧拉角（除偏航角（yaw））。
6. 数据融合
加速度传感器的静态稳定性更好，但在运动时其数据相对不可靠，而陀螺仪的动态稳定性更好，但在静止时数据相对不可靠。所以，可以通过加速度传感器的输出来修正陀螺仪的漂移误差。这里采用Mahony互补滤波算法来实现。其原理框图如下：
计算过程如下：
- 求其实就是加速度传感器的值（）经过修正后的值（这里没有空速传感器可以认为修正量为0），即。
- 归一化
- 由四元数求重力加速度
- 求偏差
- PI控制器消偏差后补偿陀螺仪
- 由角速度求四元数
注：
- 以上计算，角速度数据单位为rad/s
- 以上计算，加速度数据单位为g
知道四元数就可很容易得到欧拉角，进而完成姿态解算。
7. 参考代码
代码直接套用上述公式即可。
#define Kp 10.0f // proportional gain governs rate of convergence to accelerometer/magnetometer
#define Ki 0.008f // integral gain governs rate of convergence of gyroscope biases
#define halfT 0.001f // half the sample period,sapmple freq=500Hz
static float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // quaternion elements representing the estimated orientation
static float exInt = 0, eyInt = 0, ezInt = 0; // scaled integral error
static float yaw = 0;
static float pitch = 0;
static float roll = 0;
void IMU_Update(float gx, float gy, float gz, float ax, float ay, float az)
{
float norm;
float vx, vy, vz;
float ex, ey, ez;
float temp0, temp1, temp2, temp3;
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); //
ax = ax / norm;
ay = ay / norm;
az = az / norm;
// estimated direction of gravity and flux (v and w)
vx = 2 * (q1q3 - q0q2);
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;
gy = gy + Kp * ey + eyInt;
gz = gz + Kp * ez + ezInt;
// integrate quaternion rate and normalise
temp0 = q0;
temp1 = q1;
temp2 = q2;
temp3 = q3;
q0 += (-temp1 * gx - temp2 * gy - temp3 * gz) * halfT;
q1 += (temp0 * gx + temp2 * gz - temp3 * gy) * halfT;
q2 += (temp0 * gy - temp1 * gz + temp3 * gx) * halfT;
q3 += (temp0 * gz + temp1 * gy - temp2 * 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;
yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2 * q2 - 2 * q3 * q3 + 1) * 57.3; // unit:degree
pitch = asin(-2 * q1 * q3 + 2 * q0 * q2) * 57.3; // unit:degree
roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 57.3; // unit:degree
}
注意：
由于加速度计对水平方向的旋转无能为力，故程序得到的偏航角（yaw）数据会一直漂移，无法得到校准，通常的解决方法是增加一个磁场传感器，来获得一个准确的水平方向角来校准陀螺仪的漂移。
参考论文（可在我的CSDN资源下载）：
A Complementary Filter for Attitude Estimation of a Fixed-Wing UAV
总结，本文介绍了基于六轴陀螺仪加速度传感器的姿态解算方法。
本文原文来自CSDN