小四轴姿态融合

1、四旋翼涉及的内容比较多，硬件电路、C程序、控制算法、上位机Java、Android等等。

2、一般而言四旋翼程序分为几个部分

1. 更新姿态角
   1. 基于加速度计、陀螺仪和磁力计的姿态融合，实时更新四元数，并由四元数解算姿态角
      1. 首先由加速度值初始化俯仰角和横滚角，由磁力计初始化航向角，由上面的姿态角初始化四元数

         init_ax=(float)(accel[0] / Accel_4_Scale_Factor);           //单位转化成重力加速单位：m/s2init_ay=(float)(accel[1] / Accel_4_Scale_Factor);init_az=(float)((accel[2]+600) / Accel_4_Scale_Factor);//进行x y轴的校准，未对z轴进行校准，参考MEMSense的校准方法init_mx =(float)1.046632*mag[0]-1.569948;init_my =(float)mag[1]-8;init_mz =(float)mag[2];init_Roll  =  atan2(init_ay, init_az);init_Pitch = -asin(init_ax);              //init_Pitch = asin(ax / 1);init_Yaw   = -atan2(init_mx*cos(init_Roll) + init_my*sin(init_Roll)*sin(init_Pitch) + init_mz*sin(init_Roll)*cos(init_Pitch),init_my*cos(init_Pitch) - init_mz*sin(init_Pitch));                       //atan2(mx, my);q0 = cos(0.5*init_Roll)*cos(0.5*init_Pitch)*cos(0.5*init_Yaw) + sin(0.5*init_Roll)*sin(0.5*init_Pitch)*sin(0.5*init_Yaw);  //wq1 = sin(0.5*init_Roll)*cos(0.5*init_Pitch)*cos(0.5*init_Yaw) - cos(0.5*init_Roll)*sin(0.5*init_Pitch)*sin(0.5*init_Yaw);  //x   绕x轴旋转是rollq2 = cos(0.5*init_Roll)*sin(0.5*init_Pitch)*cos(0.5*init_Yaw) + sin(0.5*init_Roll)*cos(0.5*init_Pitch)*sin(0.5*init_Yaw);  //y   绕y轴旋转是pitchq3 = cos(0.5*init_Roll)*cos(0.5*init_Pitch)*sin(0.5*init_Yaw) - sin(0.5*init_Roll)*sin(0.5*init_Pitch)*cos(0.5*init_Yaw);  //z   绕z轴旋转是YawYaw = init_Yaw * 57.3;

2. 归一化各个传感器参数，根据磁力计测量数据利用四元数矩阵得到地理坐标系下磁场矢量（测量值），计算地理坐标系下的磁场矢量（参考值）

   //normalise the measurementsnorm = 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 fluxhx = 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);//计算地理坐标系下的磁场矢量bxyz（参考值）bx = sqrtf((hx*hx) + (hy*hy));bz = hz; 

3. 根据四元数矩阵，将地理坐标系下加速度矢量变换到飞行器坐标下（参考值）

    //estimated direction of gravity and flux (v and w)，下面这个是从世界坐标系到飞行器坐标系的转换公式(转置矩阵)//地理坐标系下加速度为(0,0,1g)vx = 2*(q1q3 -q0q2);vy = 2*(q0q1 +q2q3);vz = q0q0 - q1q1 -q2q2 + q3q3;

4. 将地理坐标系下磁场矢量（参考值）通过四元数矩阵变换到飞行器坐标下

   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);

5. 将加速度的测量矢量与参考矢量做叉积，把磁场的测量矢量与参考矢量做叉积，得到加速度误差和磁场误差，用于修正陀螺

   // error is sum of cross product between reference direction of fields and direction measured by sensorsex = (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);

6. 误差积分，陀螺仪积分

   // integral error scaled integral gainexInt = exInt + ex*Ki;eyInt = eyInt + ey*Ki;ezInt = ezInt + ez*Ki;// adjusted gyroscope measurementsgx = gx + Kp*ex + exInt;gy = gy + Kp*ey + eyInt;gz = gz + Kp*ez + ezInt;

7. 龙格-库塔法更新四元数

   // 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;

8. 归一化四元数，由四元数计算姿态角

   // normalise quaternionnorm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);q0 = q0 * norm;       //wq1 = q1 * norm;       //xq2 = q2 * norm;       //yq3 = q3 * norm;       //zPitch = asin(-2*q1*q3 + 2*q0*q2) * 57.3; //俯仰角，绕y轴转动 Roll  = atan2(2*q2*q3 + 2*q0*q1,-2*q1*q1 - 2*q2*q2 + 1) * 57.3; //滚动角，绕x轴转动Yaw   = atan2(2*q1*q2 + 2*q0*q3,-2*q2*q2 - 2*q3*q3 + 1) * 57.3;  //偏航角，绕z轴转?




四元数矩阵

2. 利用扩展卡尔曼滤波器对加速度计和磁力计的观测值来修正陀螺积分得到的姿态角

 

2. 在这后面还有位置控制和姿态控制
1. 姿态控制要实现实时精确控制四旋翼飞行器的飞行姿态，首先通过航姿参考系统检测其三个轴向的欧拉角度和角速率，然后对其实施闭环控制，使飞行器达到期望的姿态
1. 位置控制

位置回路控制律设计是为了使四旋翼无人飞行器可以快速、准确的跟踪给定轨迹。位置回路控制算法首先需要计算得到所需姿态角度，然后由姿态控制回路计算得出给定姿态角度，实现位置回路跟踪控制。所以，外环为位置环、内环为姿态环