用的是如下算法:
float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // quaternion elements representing the estimated orientation
float exInt = 0, eyInt = 0, ezInt = 0; // scaled integral error
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; //½«Îó²îPIºó²¹³¥µ½ÍÓÂÝÒÇ£¬¼´²¹³¥ÁãµãƯÒÆ
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt; //ÕâÀïµÄgzÓÉÓÚûÓй۲âÕß½øÐнÃÕý»á²úÉúƯÒÆ£¬±íÏÖ³öÀ´µÄ¾ÍÊÇ»ý·Ö×ÔÔö»ò×Ô¼õ
// 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;
}
1.当设定Kp 1.5f,Ki 0.01f,halfT 0.001f 的时候;
输出的四元数会在一段范围内波动,在范围之间由慢到快,再到慢地跳动;
2.当设定Kp 50f 时;
输出的四元数会比较稳定,看上去正常,把MPU6050转动的时候,他需要一个时间去变化到某个值。
我想请教各路大神,Kp Ki的意义是啥呢?还有halfT的值也不知道怎么去确定的,因为我也不知道姿态解算速度是如何获得的?最后就是,Kp的值设定这么大,肯定有问题,那为啥小值得时候不能良好的输出呢?求解答啊!
这是源码,求解答!