人体姿态捕获姿态求解

1165530693

大家好，现在在做基于惯性传感器的人体姿态捕获研究，上位机已经写好，底层算法是参考的四轴解算姿态的程序，但是一般的俯仰角范围都是-90-90度之间的，达不到我想要的全角度范围，我想要的俯仰角是在0-360度之间变化，不知道大家有什么办法。
注：解算姿态采用的是官方的互补滤波算法：
void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
{
   float norm, halfT;
   float hx, hy, hz, bz, by;
   float vx, vy, vz, wx, wy, wz;
   float ex, ey, ez;
   float Pitch, Roll, Yaw;
    unsigned char tmp[8];
        short int temp[3],test[2];
        CanTxMsg TxMessage;

   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;
         

   //normalise the measurements
   norm = invSqrt(ax*ax + ay*ay + az*az);      
   ax = ax * norm;
   ay = ay * norm;
   az = az * norm;
   norm = invSqrt(mx*mx + my*my + mz*mz);         
   mx = mx * norm;
   my = my * norm;
   mz = mz * norm;         
        
/*´Ó»úÌå×ø±êϵµÄµç×ÓÂÞÅ̲⵽µÄʸÁ¿×ª³ÉµØÀí×ø±êϵϵĴų¡Ê¸Á¿hxyz£¨²âÁ¿Öµ£©£¬ÏÂÃæÕâ¸öÊÇ´Ó·ÉÐÐÆ÷×ø±êϵµ½ÊÀ½ç×ø±êϵµÄת»»¹«Ê½*/
   //compute reference direction of flux
   hx = 2*mx*(0.5 - q2q2 - q3q3) + 2*my*(q1q2 - q0q3) + 2*mz*(q1q3 + q0q2);
   hy = 2*mx*(q1q2 + q0q3) + 2*my*(0.5 - q1q1 - q3q3) + 2*mz*(q2q3 - q0q1);
   hz = 2*mx*(q1q3 - q0q2) + 2*my*(q2q3 + q0q1) + 2*mz*(0.5 - q1q1 - q2q2);

//   bx = sqrtf((hx*hx) + (hy*hy));
   by = sqrtf((hx*hx) + (hy*hy));
   bz = hz;        
   
   // estimated direction of gravity and flux (v and w)£¬ÏÂÃæÕâ¸öÊÇ´ÓÊÀ½ç×ø±êϵµ½·ÉÐÐÆ÷×ø±êϵµÄת»»¹«Ê½(תÖþØÕó)
   vx = 2*(q1q3 - q0q2);
   vy = 2*(q0q1 + q2q3);
   vz = q0q0 - q1q1 - q2q2 + q3q3;


//   wx = 2*bx*(0.5 - q2q2 - q3q3) + 2*bz*(q1q3 - q0q2);
//   wy = 2*bx*(q1q2 - q0q3) + 2*bz*(q0q1 + q2q3);
//   wz = 2*bx*(q0q2 + q1q3) + 2*bz*(0.5 - q1q1 - q2q2);
   wx = 2*by*(q1q2 + q0q3) + 2*bz*(q1q3 - q0q2);
   wy = 2*by*(0.5 - q1q1 - q3q3) + 2*bz*(q0q1 + q2q3);
   wz = 2*by*(q2q3 - q0q1) + 2*bz*(0.5 - q1q1 - q2q2);

   ex = (ay*vz - az*vy) + (my*wz - mz*wy);
   ey = (az*vx - ax*vz) + (mz*wx - mx*wz);
   ez = (ax*vy - ay*vx) + (mx*wy - my*wx);
   
//   // integral error scaled integral gain
//   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;

   halfT=GET_NOWTIME();                //µÃµ½Ã¿´Î×Ë̬¸üеÄÖÜÆÚµÄÒ»°ë
   
   if(ex != 0.0f && ey != 0.0f && ez != 0.0f)      //ºÜ¹Ø¼üµÄÒ»¾ä»°£¬Ô­Ë㷨ûÓÐ
   {
      // integral error scaled integral gain
      exInt = exInt + ex*Ki * halfT;                           //³ËÒÔ²ÉÑùÖÜÆÚµÄÒ»°ë
      eyInt = eyInt + ey*Ki * halfT;
      ezInt = ezInt + ez*Ki * halfT;
      // adjusted gyroscope measurements
      gx = gx + Kp*ex + exInt;
      gy = gy + Kp*ey + eyInt;
      gz = gz + Kp*ez + ezInt;
   }         

   // 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 = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
   q0 = q0 * norm;       //w
   q1 = q1 * norm;       //x
   q2 = q2 * norm;       //y
   q3 = q3 * norm;       //z

        Yaw   = -atan2(2*q1*q2 - 2*q0*q3, -2 * q1 * q1 - 2 * q3 * q3 + 1) * 57.295780;  //          0<Yaw<360
    //if(Yaw < 0 ){Yaw = Yaw + 180;}
        //if(Yaw > 180 ){Yaw = Yaw - 180;}
         Pitch = asin(2*q2*q3 + 2*q0*q1) * 57.295780;    //  -90<Pitch<90
    Roll  = -atan2(-2*q0*q2 + 2*q1*q3, -2 * q1 * q1 - 2 * q2* q2 + 1) * 57.295780; //¹ö¶¯½Ç£¬ÈÆyÖáת¶¯       -180<Roll<180
}
其它不多说了，上上位机图片，上位机用vc2012写的，通过tcp协议与wifi通讯，接收通过wifi转发的传感器数据。
现在主要是姿态求解问题，求大神支招。

一个人·

为啥是骷髅，不是个妹子。

vc9181

问题和2楼一样

LeoJun0614

放个二楼的头像，棒棒哒

sunnykai

人家说的是姿势，光头像不够