传感器技术 > 详情

简单的mems陀螺仪姿态算法介绍

发布时间:2024-01-31 发布时间:
|

    测试传感器:MPU9250,九轴传感器,其中有三个轴就是陀螺仪的三个方向角速度。

  陀螺仪在每个采样点获得:采样时刻(单位微妙),XYZ三个方向的角速度(单位弧度/秒),记为:wx, wy, wz。陀螺仪静止时,wx, wy, wz也是有读数的,这就是陀螺仪的零漂。

  实验一:将陀螺仪绕X轴旋转时,只有wx有读数;将陀螺仪绕Y轴旋转时,只有wy有读数;将陀螺仪绕Z轴旋转时,只有wz有读数;

  实验二:将陀螺仪绕XY面上的轴旋转,wz读数为零,即与旋转轴垂直的轴上的角速度为零。

  因为陀螺仪采样率很高(1000Hz),通过瞬时读数计算姿态,可以看做:先绕X轴旋转,再绕Y轴旋转,再绕Z轴旋转。

  下面这段代码实现了一个简单的陀螺仪姿态算法,开机并静置几十秒后,拿着陀螺仪旋转,十几分钟内姿态是正确的,之后由于积分累积,误差就越来越大了。
        // 参数说明:

// sampleTS   : 采样时刻,单位:微秒
// wx, wy, wz :陀螺仪采样,单位:弧度/秒
void GyroExperiment(uint64_t sampleTS, float wx, float wy, float wz)
{
    // 传感器启动时刻
    static uint64_t s_lastTIme = 0;
    staTIc uint64_t s_lastlog = 0;
    if(s_lastTIme == 0){
        s_lastTIme = sampleTS;
        s_lastlog = sampleTS;
        return;
    }
 
    // 采用启动后5秒-35秒的采用平均值作为陀螺仪零漂
    // 在此期间,应保持陀螺仪静止
    static float s_wx = 0, s_wy = 0, s_wz = 0;    // 陀螺仪零漂
    static uint64_t s_elapsed = 0;
    if(s_elapsed < 35000000){
        static int s_SampleCnt = 0;
        if(s_elapsed > 5000000){
            s_wx += (wx - s_wx) / (s_SampleCnt + 1);
            s_wy += (wy - s_wy) / (s_SampleCnt + 1);
            s_wz += (wz - s_wz) / (s_SampleCnt + 1);
            s_SampleCnt++;
        }
        s_elapsed += (sampleTS - s_lasttime);
        s_lasttime = sampleTS;
    }
 
    // 初始姿态,采用三个轴向量表示
    static float Xx=1,Xy=0,Xz=0;    // X轴
    static float Yx=0,Yy=1,Yz=0;    // Y轴
    static float Zx=0,Zy=0,Zz=1;    // Z轴
 
    // 根据陀螺仪读数计算三个轴的旋转量
    float interval = (sampleTS - s_lasttime) / 1e6;
    float rx = (wx - s_wx) * interval;
    float ry = (wy - s_wy) * interval;
    float rz = (wz - s_wz) * interval;
 
    // 分别构造绕三个轴旋转的四元数
    float cos,sin;
    cos = cosf(rx/2); sin = sinf(rx/2); Quaternion qx(cos, Xx * sin, Xy * sin, Xz * sin);
    cos = cosf(ry/2); sin = sinf(ry/2); Quaternion qy(cos, Yx * sin, Yy * sin, Yz * sin);
    cos = cosf(rz/2); sin = sinf(rz/2); Quaternion qz(cos, Zx * sin, Zy * sin, Zz * sin);
 
    // 依次旋转三个轴向量
    Quaternion q = qx*qz*qy; q.normalize(); Quaternion qi = q.inverse();
    Quaternion QX(0, Xx, Xy, Xz); QX = q*QX*qi; QX.normalize(); Xx = QX.q2; Xy = QX.q3; Xz = QX.q4;        // 旋转X轴;
    Quaternion QY(0, Yx, Yy, Yz); QY = q*QY*qi; QY.normalize(); Yx = QY.q2; Yy = QY.q3; Yz = QY.q4;        // 旋转Y轴;
    Quaternion QZ(0, Zx, Zy, Zz); QZ = q*QZ*qi; QZ.normalize(); Zx = QZ.q2; Zy = QZ.q3; Zz = QZ.q4;        // 旋转Z轴;
 
    // 每1秒输出一次姿态数据
    s_lasttime = sampleTS;
    if(sampleTS - s_lastlog > 1000000){
        console->printf("attitude: [%f, %f, %f]; [%f, %f, %f]; [%f, %f, %f]\n", Xx, Xy, Xz, Yx, Yy, Yz, Zx, Zy, Zz);
        s_lastlog = sampleTS;
    }
}

『本文转载自网络,版权归原作者所有,如有侵权请联系删除』

热门文章 更多
工业振动加速度传感器的实际应用是什么