電子發(fā)燒友App
? ? 測試傳感器：MPU9250，九軸傳感器，其中有三個軸就是陀螺儀的三個方向角速度。
陀螺儀在每個采樣點獲得：采樣時刻（單位微妙），XYZ三個方向的角速度（單位弧度/秒），記為：wx， wy， wz。陀螺儀靜止時，wx， wy， wz也是有讀數(shù)的，這就是陀螺儀的零漂。
實驗一：將陀螺儀繞X軸旋轉(zhuǎn)時，只有wx有讀數(shù)；將陀螺儀繞Y軸旋轉(zhuǎn)時，只有wy有讀數(shù)；將陀螺儀繞Z軸旋轉(zhuǎn)時，只有wz有讀數(shù)；
實驗二：將陀螺儀繞XY面上的軸旋轉(zhuǎn)，wz讀數(shù)為零，即與旋轉(zhuǎn)軸垂直的軸上的角速度為零。
因為陀螺儀采樣率很高（1000Hz），通過瞬時讀數(shù)計算姿態(tài)，可以看做：先繞X軸旋轉(zhuǎn)，再繞Y軸旋轉(zhuǎn)，再繞Z軸旋轉(zhuǎn)。
下面這段代碼實現(xiàn)了一個簡單的陀螺儀姿態(tài)算法，開機并靜置幾十秒后，拿著陀螺儀旋轉(zhuǎn)，十幾分鐘內(nèi)姿態(tài)是正確的，之后由于積分累積，誤差就越來越大了。
? ? ? ??// 參數(shù)說明：
// 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秒的采用平均值作為陀螺儀零漂
? ? // 在此期間，應(yīng)保持陀螺儀靜止
? ? 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;
? ? }
?
? ? // 初始姿態(tài)，采用三個軸向量表示
? ? 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軸
?
? ? // 根據(jù)陀螺儀讀數(shù)計算三個軸的旋轉(zhuǎn)量
? ? float interval = (sampleTS - s_lasttime) / 1e6;
? ? float rx = (wx - s_wx) * interval;
? ? float ry = (wy - s_wy) * interval;
? ? float rz = (wz - s_wz) * interval;
?
? ? // 分別構(gòu)造繞三個軸旋轉(zhuǎn)的四元數(shù)
? ? 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);
?
? ? // 依次旋轉(zhuǎn)三個軸向量
? ? 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;? ? ? ? // 旋轉(zhuǎn)X軸;
? ? Quaternion QY(0, Yx, Yy, Yz); QY = q*QY*qi; QY.normalize(); Yx = QY.q2; Yy = QY.q3; Yz = QY.q4;? ? ? ? // 旋轉(zhuǎn)Y軸;
? ? Quaternion QZ(0, Zx, Zy, Zz); QZ = q*QZ*qi; QZ.normalize(); Zx = QZ.q2; Zy = QZ.q3; Zz = QZ.q4;? ? ? ? // 旋轉(zhuǎn)Z軸;
?
? ? // 每1秒輸出一次姿態(tài)數(shù)據(jù)
? ? 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;
? ? }
}
工商網(wǎng)監(jiān)
評論