四元数姿态解算

四元数姿态解算——互补滤波
六轴

float q0=1, q1=0, q2=0, q3=0;
float exInt, eyInt, ezInt;
//*****************************************6轴融合*****************************************************
#define Kp 0.2f//2.0f //加速度权重，越大则向加速度测量值收敛越快
#define Ki 0.001f //误差积分增益
#define halfT (dt*0.5)
void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
{
	float norm;
	float vx, vy, vz;
	float ex, ey, ez; 

         filted.roll_d=gy*RtoA;
         filted.pitch_d=gx*RtoA;
         filted.yaw_d=gz*RtoA;
 
	//加计值归一化处理normalise the measurements
	norm = sqrtf(ax*ax + ay*ay + az*az);      
	ax = ax / norm;
	ay = ay / norm;
	az = az / norm;      

	//建立飞控坐标系estimated direction of gravity
	vx = 2*(q1*q3 - q0*q2);
	vy = 2*(q0*q1 + q2*q3);
	vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;

	//坐标系和重力叉积运算error is sum of cross product between reference direction of field and direction measured by sensor
	ex = (ay*vz - az*vy);
	ey = (az*vx - ax*vz);
	ez = (ax*vy - ay*vx);

	//比例运算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;
	//计算四元数 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 = sqrtf(q0*q0 + q1*q1 + q2*q2 + q3*q3);	
	q0 = q0 / norm;
	q1 = q1 / norm;
	q2 = q2 / norm;
	q3 = q3 / norm;
	
	//计算欧拉角
	filted.roll	=	-asinf(-2*q1*q3 + 2*q0*q2)*57.30f;//正负90度
	filted.pitch	=	-atan2f(2*q2*q3 + 2*q0*q1, -2*q1*q1-2*q2*q2 + 1)*57.30f;//正负180度
    filted.yaw= atan2f(2*q1*q2 - 2*q0*q3, -2 * q1 * q1 - 2 * q3 * q3 + 1) * 57.30f;  //乘以57.30是为了将弧度转化为角度
	//Current_yaw		=	Current_yaw-gz*Gyro_angle_scale;
        filted.cos_pitch=cosf(filted.pitch*AtoR);
        filted.sin_pitch=sinf(filted.pitch*AtoR);
        filted.cos_roll=cosf(filted.roll*AtoR);
        filted.sin_roll=sinf(filted.roll*AtoR);
        filted.cos_yaw=cosf(filted.yaw*AtoR);
        filted.sin_yaw=sinf(filted.yaw*AtoR);
}

九轴

//*********************************************9轴融合******************************************************
 void AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) 
 {
    float norm;
    float hx, hy, hz, bz,bx;// by;
    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;
           
    
    
         filted.roll_d=gy*57.295779514719953173585430909526f;
         filted.pitch_d=-gx*57.295779514719953173585430909526f;
         filted.yaw_d=-gz*57.295779514719953173585430909526;
         
 /*归一化测量值，加速度计和磁力计的单位是什么都无所谓，因为它们在此被作了归一化处理*/        
    norm = sqrtf(ax*ax + ay*ay + az*az);       
    ax = ax / norm;
    ay = ay / norm;
    az = az / norm;
    norm = sqrtf(mx*mx + my*my + mz*mz);          
    mx = mx / norm;
    my = my / norm;
    mz = mz / norm;         
         
 /*从机体坐标系的电子罗盘测到的矢量转成地理坐标系下的磁场矢量h-xyz（测量值），下面这个是从飞行器坐标系到地理坐标系的转换公式*/
    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);

 /*计算地理坐标系下的磁场矢量b-xyz（参考值）。因为地理地磁水平夹角，我们已知是0度（抛去磁偏角的因素，固定向北），
 我定义b-y指向正北，所以by=某值，bx=0但地理参考地磁矢量在垂直面上也有分量bz，地球上每个地方都是不一样的。
 我们无法得知，也就无法用来融合（有更适合做垂直方向修正融合的加速度计），所以直接从测量值hz上复制过来，bz=hz。
 磁场水平分量，参考值和测量值的大小应该是一致的(bx*bx) + (by*by)) = ((hx*hx) + (hy*hy))。
 因为bx=0，所以就简化成(by*by)  = ((hx*hx) + (hy*hy))。可算出by。下面可以修改b-y和b-x那个轴指向正北。*/
 	bx = sqrtf((hx*hx) + (hy*hy));
 //   by = sqrtf((hx*hx) + (hy*hy));
 	bz = hz;        
     
 //下面的公式是从地理坐标系到飞控坐标系的转化
 	vx = 2*(q1q3 - q0q2);
 	vy = 2*(q0q1 + q2q3);
 	vz = q0q0 - q1q1 - q2q2 + q3q3;

 /*我们把地理坐标系上的磁场矢量b-xyz，转到机体上来w-xyz。
 因为bx/y=0，所以所有涉及到bx/y的部分都被省略了，这根据自己定义那个轴指北有关。
 类似上面重力v-xyz的推算，因为重力g的az=1，ax=ay=0，所以上面涉及到gxgy的部分也被省略了
 你可以看看两个公式：w-xyz的公式，把by换成ay（0），把bz换成az（1），就变成了v-xyz的公式了（其中q0q0+q1q1+q2q2+q3q3=1）。*/
 //x轴对准北方
 	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);
 //y轴对准北方
 //   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);

 //积分计算，乘以采样周期的一半

 	exInt = exInt + ex*Ki; //* halfT;			  
 	eyInt = eyInt + ey*Ki;// * halfT;
 	ezInt = ezInt + ez*Ki;// * halfT;
 			
 //补偿陀螺仪PI计算
 	gx = gx + Kp*ex + exInt;
 	gy = gy + Kp*ey + eyInt;
 	gz = gz + Kp*ez + ezInt;
  

 //更新四元数
 	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;  
         
 //四元数归一化
 	norm = sqrtf(q0*q0 + q1*q1 + q2*q2 + q3*q3);
 	q0 = q0 / norm;       //w
 	q1 = q1 / norm;       //x
 	q2 = q2 / norm;       //y
 	q3 = q3 / norm;       //z
 		
          

 	filted.yaw= atan2f(2*q1*q2 - 2*q0*q3, -2 * q1 * q1 - 2 * q3 * q3 + 1) * 57.30f;  //乘以57.30是为了将弧度转化为角度
 	filted.pitch= -asinf(2*q2*q3 + 2*q0*q1) * 57.30f;																	 //正负180度	
 	filted.roll= -atan2f(-2*q0*q2 + 2*q1*q3, -2 * q1 * q1 - 2 * q2* q2 + 1) * 57.30f;
 	
        filted.yaw_mag=atan2f(my,mx)*RtoA;
 	
 }

四元数姿态解算——梯度下降法`

void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
    float recipNorm;
    float s0, s1, s2, s3;
    float qDot1, qDot2, qDot3, qDot4;
    float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
    
#define sampleFreq  200.0f      // sample frequency in Hz
#define betaDef     0.1f        // 2 * proportional gain



    static float beta = betaDef;                              // 2 * proportional gain (Kp)
    static float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;  // quaternion of sensor frame relative to auxiliary fram

    // Rate of change of quaternion from gyroscope
    qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
    qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
    qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
    qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);

    // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
    if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {

        // Normalise accelerometer measurement
        recipNorm = 1/sqrt(ax * ax + ay * ay + az * az);
        ax *= recipNorm;
        ay *= recipNorm;
        az *= recipNorm;   

        // Auxiliary variables to avoid repeated arithmetic
        _2q0 = 2.0f * q0;
        _2q1 = 2.0f * q1;
        _2q2 = 2.0f * q2;
        _2q3 = 2.0f * q3;
        _4q0 = 4.0f * q0;
        _4q1 = 4.0f * q1;
        _4q2 = 4.0f * q2;
        _8q1 = 8.0f * q1;
        _8q2 = 8.0f * q2;
        q0q0 = q0 * q0;
        q1q1 = q1 * q1;
        q2q2 = q2 * q2;
        q3q3 = q3 * q3;

        // Gradient decent algorithm corrective step
        s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
        s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
        s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
        s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
        recipNorm = 1/sqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
        s0 *= recipNorm;
        s1 *= recipNorm;
        s2 *= recipNorm;
        s3 *= recipNorm;

        // Apply feedback step
        qDot1 -= beta * s0;
        qDot2 -= beta * s1;
        qDot3 -= beta * s2;
        qDot4 -= beta * s3;
    }

    // Integrate rate of change of quaternion to yield quaternion
    q0 += qDot1 * (1.0f / sampleFreq);
    q1 += qDot2 * (1.0f / sampleFreq);
    q2 += qDot3 * (1.0f / sampleFreq);
    q3 += qDot4 * (1.0f / sampleFreq);

    // Normalise quaternion
    recipNorm = 1/sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    q0 *= recipNorm;
    q1 *= recipNorm;
    q2 *= recipNorm;
    q3 *= recipNorm;
    
    
    //计算欧拉角
    filted.roll	=	-asinf(-2*q1*q3 + 2*q0*q2)*57.30f;//正负90度
    filted.pitch	=	-atan2f(2*q2*q3 + 2*q0*q1, -2*q1*q1-2*q2*q2 + 1)*57.30f;//正负180度
     //atan2f(2*q1*q2 - 2*q0*q3, -2 * q1 * q1 - 2 * q3 * q3 + 1) * 57.30f;  //乘以57.30是为了将弧度转化为角度
    //Current_yaw		=	Current_yaw-gz*Gyro_angle_scale;
    filted.cos_pitch=cosf(filted.pitch*AtoR);
    filted.sin_pitch=sinf(filted.pitch*AtoR);
    filted.cos_roll=cosf(filted.roll*AtoR);
    filted.sin_roll=sinf(filted.roll*AtoR);
    filted.cos_yaw=cosf(filted.yaw*AtoR);
    filted.sin_yaw=sinf(filted.yaw*AtoR);
     filted.roll_d=gy*RtoA;
     filted.pitch_d=gx*RtoA;
     filted.yaw_d=gz*RtoA;
     if(fabsf(filted.yaw_d)>0.15){
      filted.yaw+=0.005*gz*RtoA;
     }
}

上文详细描述了由Madgwick提出四元数AHRS姿态解算和IMU姿态解算方法，如需转载，请勿用于商业目的，并注明：该文章出自圆点博士无人机www.bspilot.com