姿态解算

最新推荐文章于 2024-04-22 17:37:28 发布

经纬的无疆

最新推荐文章于 2024-04-22 17:37:28 发布

阅读量4.4k

点赞数 7

分类专栏：自研飞控

本文链接：https://blog.csdn.net/awujiang/article/details/94172654

版权

自研飞控专栏收录该内容

4 篇文章 10 订阅

订阅专栏

姿态解算

准确快速的姿态解算是无人机姿态控制和位置控制的基础。现在常用的姿态解算方法主要有互补滤波、扩展卡尔曼滤波。相比较两种方法，互补滤波较为简单，容易调试，我这里也是选择Mahony互补滤波方法进行姿态解算。

无人机姿态

无人机姿态角一共有三个，分别是滚转、俯仰、偏航，分别是绕无人机的X轴、Y轴、Z轴旋转的角度。知道无人机的实时姿态情况是进行下一步控制的基础，很难想象不知道无人机的姿态角度还能控制它平稳的飞行。

机体坐标系和大地坐标系

大地坐标系比较容易理解，我们选取正北、正东、正上构成这个坐标系的X、Y、Z轴的正方向。我们用R表示大地坐标系。为了编程和控制的方便，我们在飞机上也构想一个坐标系，称为机体坐标系，分别选取滚转、俯仰、偏航所绕的轴作为x、y、z的正方向，我们用r表示机体坐标系。
通过传感器能够得到飞机相对于r坐标系的姿态，但是飞机在高空中飞来飞去，坐标系是不断变化的，我们很难肉眼观察到它相对于机体坐标系的角度。但是如果能够知道飞机的姿态角相对于大地坐标系的值，我们就可以很直接看出来或者显示出来，方便后面的控制。

姿态表示方法

姿态表示的方法常见的有三种：四元数、欧拉角、旋转矩阵。我们所说的滚转、俯仰、偏航就是用欧拉角来表示的。~~（这里放图不是很方便，后面再放图来形象表示一下吧）~~

算法	优点	缺点	其他
四元数	直接求解四元数微分方程，四个未知参数，计算量小	漂移比较严重	可用于变换时保存姿态角
欧拉角	直接求解欧拉微分方程，得到pitch、roll、yaw，直观容易理解	求解方程困难，当pitch接近90度时候会出现退化现象
旋转矩阵	求解姿态矩阵微分方程，避免退化现象	参数量多	可用于向量的表示和变换

姿态解算

姿态解算是根据IMU数据求解无人机在空中的姿态，也叫做IMU数据融合。得到陀螺仪的角速率后（后面再写一篇博客说说怎么进行IMU传感器数据的获取和校准），按照道理可以用陀螺仪的角速率积分得到角度，因为陀螺仪为三轴传感器，可以得到滚转、俯仰、偏航的角度。但是由于积分的作用，会随着时间产生非常大的积分误差。那如何尽量消除这个误差呢？
IMU模块一般都不会仅仅包含一个陀螺仪传感器，至少还会有一个加速度计。而加速度计通过飞机的受力计算可以得到一个角度。（但加速度计无法感知水平方向的角度，所以必须得加上磁力计才能修正偏航角）那这两个角度更应该相信谁呢？由于飞机在空中飞行时，难免会产生振动，但加速度计对震动比较敏感，陀螺仪却不敏感。所以我们可以说，要长时间相信陀螺仪的数据，偶尔相信加速度计和磁力计的数据，也就是说用加速度计和磁力计的数据来修正陀螺仪带来的积分误差。

四元数和欧拉角在姿态解算中使用

姿态解算的核心在于旋转，旋转的表示方法主要有：四元数、旋转矩阵、欧拉角。其中，欧拉角最为直观，用来表示姿态角具体为多少度。旋转矩阵表示向量最为方便，而四元数参数较少，更为方便表示旋转，用于保存变换中的飞机姿态。

主要代码分析

/* ! Using accelerometer, sense the gravity vector. */
/* ! Using magnetometer, sense yaw. */
static void NonlinearSO3AHRSinit(float ax,  float ay,  float az,  float mx,  float my,  float mz)
{
  float initialRoll, initialPitch;
  float cosRoll, sinRoll, cosPitch, sinPitch;
  float magX, magY;
  float initialHdg, cosHeading, sinHeading;
  
  initialRoll = atan2(-ay,-az);     //求出Roll，Pitch用加速度表示方法
  initialPitch = atan2(ax, -az);
  
  cosRoll = cosf(initialRoll);      //求余弦正弦
  sinRoll = sinf(initialRoll);
  cosPitch = cosf(initialPitch);
  sinPitch = sinf(initialPitch);
  
   //其中一种的旋转方法引起Z轴的角度变化情况
  magX = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch;
  
  magY = my * cosRoll - mz * sinRoll;
  
  initialHdg = atan2f(-magY, magX);
  
  //具体公式参见全权老师的《多旋翼无人机设计与控制》 103页
  cosRoll = cosf(initialRoll * 0.5f);
  sinRoll = sinf(initialRoll * 0.5f);
  
  cosPitch = cosf(initialPitch * 0.5f);
  sinPitch = sinf(initialPitch * 0.5f);
  
  cosHeading = cosf(initialHdg * 0.5f);
  sinHeading = sinf(initialHdg * 0.5f);
  //四元数q0,q1,q2,q3表示
  q0 = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading;
  q1 = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading;
  q2 = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading;
  q3 = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading;
  q0q0=1;
  // auxillary variables to reduce number of repeated operations, for 1st pass
  q0q0 = q0 * q0;
  q0q1 = q0 * q1;
  q0q2 = q0 * q2;
  q0q3 = q0 * q3;
  q1q1 = q1 * q1;
  q1q2 = q1 * q2;
  q1q3 = q1 * q3;
  q2q2 = q2 * q2;
  q2q3 = q2 * q3;
  q3q3 = q3 * q3;
}

/*
    使用的是Mahony互补滤波算法，没有使用Kalman滤波算法
    改算法是直接参考pixhawk飞控的算法，可以在Github上看到出处
  https://github.com/hsteinhaus/PX4Firmware/blob/master/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp   
*/
static void NonlinearSO3AHRSupdate(float gx,  float gy,  float gz,  float ax,  float ay,  float az,  float mx,  float my,  float mz,  float twoKp,  float twoKi,  float dt)
{
    float recipNorm;
    float halfex = 0.0f,  halfey = 0.0f,  halfez = 0.0f;
    
     // Make filter converge to initial solution faster
    // This function assumes you are in static position.
    // WARNING : in case air reboot, this can cause problem. But this is very unlikely happen.
    if(bFilterInit == 0) 
    {
        NonlinearSO3AHRSinit(ax,ay,az,mx,my,mz);
        bFilterInit = 1;
    }
    //增加一个条件：  加速度的模量与G相差不远时。 0.75*G < normAcc < 1.25*G
    /*Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)*/
    /* 如果加速计各轴的数均是0，那么忽略该加速度数据。否则在加速计数据归一化处理的时候，会导致除以0的错误 */
     if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)))
     {
         float halfvx, halfvy, halfvz;
         // Normalise accelerometer measurement
        //归一化，得到单位加速度
         recipNorm = invSqrt(ax * ax + ay * ay + az * az);
/*把加速度计的数据进行归一化处理。其中invSqrt是平方根的倒数，使用平方根的倒数而不是直接使用平方
根的原因是使得下面的ax，ay，az的运算速度更快,通过归一化处理后，ax，ay，az的数值范围变成-1到+1 */
         ax *= recipNorm;
         ay *= recipNorm;
         az *= recipNorm;
         // Estimated direction of gravity and magnetic field
         //根据当前四元数的姿态值来估算出各重力分量
         halfvx = q1q3 - q0q2;
         halfvy = q0q1 + q2q3;
         halfvz = q0q0 - 0.5f + q3q3;
         // Error is sum of cross product between estimated direction and measured direction of field vectors
         halfex += ay * halfvz - az * halfvy;
         halfey += az * halfvx - ax * halfvz;
         halfez += ax * halfvy - ay * halfvx;
     }
     ! If magnetometer measurement is available, use it.
   if(!((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)))
   {
      float hx, hy, hz, bx, bz;
     float halfwx, halfwy, halfwz;
      
      // Normalise magnetometer measurement  归一化处理
      recipNorm = invSqrt(mx * mx + my * my + mz * mz);
      mx *= recipNorm;
      my *= recipNorm;
      mz *= recipNorm;
      
      // Reference direction of Earth's magnetic field 旋转矩阵中磁力计  这个不是很明白!!
      hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
      hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
      hz = 2.0f * mx * (q1q3 - q0q2) + 2.0f * my * (q2q3 + q0q1) + 2.0f * mz * (0.5f - q1q1 - q2q2);
      bx = sqrt(hx * hx + hy * hy);
      bz = hz;
      
      // Estimated direction of magnetic field   
     //根据当前四元数的姿态值来估算出各地磁分量Wx，Wy，Wz
      halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
      halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
      halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);
     
     // Error is sum of cross product between estimated direction and measured direction of field vectors
      halfex += (my * halfwz - mz * halfwy);
      halfey += (mz * halfwx - mx * halfwz);
      halfez += (mx * halfwy - my * halfwx);
   }
    
    // Apply feedback only when valid data has been gathered from the accelerometer or magnetometer
    if(halfex != 0.0f  &&  halfey != 0.0f  &&  halfez != 0.0f)
    {
      // Compute and apply integral feedback if enabled
      if(twoKi > 0.0f)
      {
        gyro_bias[0]  +=  twoKi * halfex * dt;	// integral error scaled by Ki
        gyro_bias[1]  +=  twoKi * halfey * dt;  //Ki*Ts*X(n)  
        gyro_bias[2]  +=  twoKi * halfez * dt;
        
        // apply integral feedback
        gx += gyro_bias[0];
        gy += gyro_bias[1];
        gz += gyro_bias[2];
      }
      else
      {
        gyro_bias[0] = 0.0f;	// prevent integral windup
        gyro_bias[1] = 0.0f;
        gyro_bias[2] = 0.0f;
      }
      // Apply proportional feedback
       gx += twoKp * halfex;    //Kp*X(n)
       gy += twoKp * halfey;  
       gz += twoKp * halfez; 
    }
    // Time derivative of quaternion. q_dot = 0.5*q\otimes omega.
    //! q_k = q_{k-1} + dt*\dot{q}
    //! \dot{q} = 0.5*q \otimes P(\omega)
      dq0 = 0.5f*(-q1 * gx - q2 * gy - q3 * gz);
      dq1 = 0.5f*(q0 * gx + q2 * gz - q3 * gy);
      dq2 = 0.5f*(q0 * gy - q1 * gz + q3 * gx);
      dq3 = 0.5f*(q0 * gz + q1 * gy - q2 * gx);
      
      q0 += dt*dq0;
      q1 += dt*dq1;
      q2 += dt*dq2;
      q3 += dt*dq3;
    // Normalise quaternion
      recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
      q0 *= recipNorm;
      q1 *= recipNorm;
      q2 *= recipNorm;
      q3 *= recipNorm;
     // Auxiliary variables to avoid repeated arithmetic
      q0q0 = q0 * q0;
      q0q1 = q0 * q1;
      q0q2 = q0 * q2;
      q0q3 = q0 * q3;
      q1q1 = q1 * q1;
      q1q2 = q1 * q2;
      q1q3 = q1 * q3;
      q2q2 = q2 * q2;
      q2q3 = q2 * q3;
      q3q3 = q3 * q3;   
}

#define so3_comp_params_Kp 1.5f
#define so3_comp_params_Ki 0.05f   //0.05f

//函数名：IMUSO3Thread(void)
//描述：姿态软件解算融合函数
//该函数对姿态的融合是软件解算
void IMUSO3Thread(void)
{
  //! Time constant
  float dt = 0.01f;   //s
  static uint32_t tPrev=0,startTime=0;	//us
  uint32_t now;
  uint8_t i;
  static uint8_t t=0;
	
   /* output euler angles */
  float euler[3] = {0.0f, 0.0f, 0.0f};	//rad
  
  float gyro[3] = {0.0f, 0.0f, 0.0f};   //rad/s
  float acc[3] = {0.0f, 0.0f, 0.0f};		//m/s^2
  float mag[3] = {0.0f, 0.0f, 0.0f};
  float Rot_matrix[9] = {1.f,  0.0f,  0.0f, 0.0f,  1.f,  0.0f, 0.0f,  0.0f,  1.f };		
  /**< init: identity matrix */

  now=HAL_GetTick();
  dt=(tPrev>0)?(now-tPrev)/1000.0f:0;
  tPrev=now;
	
  ReadIMUSensorHandle(); //得到IMU数据，注意单位

  gyro[0] = imu.gyro[0];
  gyro[1] = imu.gyro[1];
  gyro[2] = imu.gyro[2];
   
  acc[0] = -imu.accb[0];
  acc[1] = -imu.accb[1];
  acc[2] = -imu.accb[2];
   
  mag[0]= imu.accg[0];
  mag[1]= imu.accg[1];
  mag[2]= imu.accg[2];
  // NOTE : Accelerometer is reversed.
  // Because proper mount of PX4 will give you a reversed accelerometer readings.
   
  NonlinearSO3AHRSupdate(gyro[0], gyro[1], gyro[2],  
                           -acc[0], -acc[1], -acc[2],
                           mag[0], mag[1], mag[2],
                           so3_comp_params_Kp,
                           so3_comp_params_Ki,
                           dt);
  // Convert q->R, This R converts inertial frame to body frame.
    Rot_matrix[0] = q0q0 + q1q1 - q2q2 - q3q3;// 11
    Rot_matrix[1] = 2.f * (q1*q2 + q0*q3);	// 12
    Rot_matrix[2] = 2.f * (q1*q3 - q0*q2);	// 13
    Rot_matrix[3] = 2.f * (q1*q2 - q0*q3);	// 21
    Rot_matrix[4] = q0q0 - q1q1 + q2q2 - q3q3;  // 22
    Rot_matrix[5] = 2.f * (q2*q3 + q0*q1);	// 23
    Rot_matrix[6] = 2.f * (q1*q3 + q0*q2);	// 31
    Rot_matrix[7] = 2.f * (q2*q3 - q0*q1);	// 32
    Rot_matrix[8] = q0q0 - q1q1 - q2q2 + q3q3;  // 33
    

    //1-2-3 Representation.
    //Equation (290)
    //Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors, James Diebel.
    // Existing PX4 EKF code was generated by MATLAB which uses coloum major order matrix. 
   euler[0] = atan2f(Rot_matrix[5], Rot_matrix[8]);	//! Roll
	euler[1] = -asinf(Rot_matrix[2]);									//! Pitch
   euler[2] = atan2f(Rot_matrix[1], Rot_matrix[0]);
   //DCM . ground to body
   for(i=0; i<9; i++)
    {
        *(&(imu.DCMgb[0][0]) + i)=Rot_matrix[i];
    }
   
   imu.rollRad=euler[0];
   imu.pitchRad=euler[1];
   imu.yawRad=euler[2];
		
	/* 直接利用陀螺仪的Z轴角速度会有很大的漂移 */	
	 Yaw_Gyro_Earth_Frame = -sin(imu.rollRad) * imu.gyro[0]*180.f/M_PI_F+
                       		cos(imu.rollRad)*sin(imu.pitchRad) * imu.gyro[1]*180.f/M_PI_F+ 
		                      cos(imu.pitchRad) * cos(imu.rollRad ) * imu.gyro[2]*180.f/M_PI_F;  //用于Yaw 角速度反馈
		
		/* 对磁力计的磁偏角进行补偿 https://blog.csdn.net/u013636775/article/details/72675148 */
		/* https://blog.csdn.net/xiaoxilang/article/details/80525236 */
		/* 计算磁力计数据在水平面 X Y轴上的投影 补偿前提是磁力计与加速度计同轴，方向正反错误需要调整*/
    imu.accg[1] = imu.accg[0] * sin(imu.pitchRad) * sin(imu.rollRad)
  + imu.accg[1] * cos(imu.pitchRad)
    - imu.accg[2] * cos(imu.rollRad) * sin(imu.pitchRad);
    imu.accg[0] = imu.accg[0] * cos(imu.rollRad)+ imu.accg[2] * sin(imu.rollRad);
		
   euler[2]= atan2((double)(imu.accg[0]),(double)(imu.accg[1]));  /* 磁力计的 X Y 轴颠倒了,在滤波前调换了一下*/
		
	 Mag_Yaw = euler[2] * 180.0f / M_PI_F; /* 磁力计计算出的偏航角 单位 deg */
	 att_angle[0] += Yaw_Gyro_Earth_Frame * dt; /* 陀螺仪积分计算出的偏航角 单位 deg */
	 /*---- 偏航角一阶互补 -----*/
	 if((Mag_Yaw>90 && att_angle[0]<-90)|| (Mag_Yaw<-90 && att_angle[0]>90))
	 {
		 att_angle[0] = -att_angle[0] * 0.99f + Mag_Yaw * 0.01f;
	 } 
   else 
	 {
		 att_angle[0] = att_angle[0] * 0.99f + Mag_Yaw * 0.01f;
	 }
   imu.yaw = att_angle[0];  /* 最后计算出的偏航角 单位 deg */
   
 /* 计算欧拉角，单位 deg/s 实际上Pitch 和Roll反了，参考Pixhawk的硬件的结果*/
   imu.roll= euler[0] * 180.0f / M_PI_F;  
   imu.pitch= euler[1] * 180.0f / M_PI_F;  
 //  imu.yaw=  euler[2] * 180.0f / M_PI_F;

  if(imu.yaw<0)
  {
     imu.yaw+=360;
  }
  else if(imu.yaw>360)
  {
     imu.yaw-=360;
  }  	
}

代码原理分析

对于四元数法的姿态求解，需要求解的就是四元数的值。方向余弦矩阵用来转换时表示向量，本来矩阵的元素都是一些三角函数，这里可以用四元数来表示矩阵的元素。最后变成求解由四元数构成的余弦矩阵。
加速度计仅仅测量的是重力加速，三轴加速度计是重力加速度的三轴分量。重力加速度的方向和大小是固定的，通过这种关系，可以得到机体坐标系和地面坐标系的角度关系。在大地坐标系R中，加速度计a输出为
$0,0,1]^{T}$
经过旋转矩阵的变换到机体坐标系r中，a变成
$v x, v y, v z]^{T}$
在机体坐标系中，加速度计的测量值为
$a x, a y, a z]^{T}$
那得到两个加速度计的向量值，我们由此可以做向量积，得到误差向量，再用误差向量来修正旋转矩阵。（这一个思想在光流传感器数据互补融合中也用到了）还有一些数学理论知识，这里打公式不太好搞，等以后慢慢更新。

经纬的无疆

关注

7
点赞
踩
53

收藏

觉得还不错? 一键收藏
打赏
1
评论
姿态解算

姿态解算       准确快速的姿态解算是无人机姿态控制和位置控制的基础。现在常用的姿态解算方法主要有互补滤波、扩展卡尔曼滤波。相比较两种方法，互补滤波较为简单，容易调试，我这里也是选择Mahony互补滤波方法进行姿态解算。无人机姿态       无人机姿态角一共有三个，分别是滚转、俯仰、偏航，分别是绕无人机的...
复制链接

扫一扫