空间姿态解算相关理解

基本知识点的理解

反三角函数的理解

• atan2(y,x) (-pi,pi) 根据象限划分
• atan(y/x) (-pi/2,pi/2)

矩阵的变换

• 左乘一个矩阵：相对于基坐标系下进行旋转或者确定新的基的方向
• 右乘一个矩阵：相对于当前坐标系下进行旋转或者确定新的基的方向

RPW相关理解

• Roll（滚转）
  
  • 绕Z轴旋转（Z轴不动）
• Pitch（俯仰）
  
  • 绕X轴旋转（X轴不动）
• Yaw（偏航）
  
  • 绕Y轴旋转（Y轴不动）

四轴保持稳定的控制方法

• 根据角度传感器得到滚转、俯仰、偏航角度，然后利用三组PID调节可以使得系统保持稳定（目标值均为0），得到的值是系统的反馈值
• 滚转
  
  • 和俯仰类似
• 俯仰
  
  • 一组电机速度相同，另一组电机一个速度增加，一个速度减少，即可实现控制
• 偏航
  
  • 一组电机速度相同且速度较大，另一组电机速度相同且较小，力矩不同，实现绕Z轴旋转

MMA7361使用

接线

• Sleep接3.3V，芯片是3.3V供电，一定要注意模块是否含有电压转换芯片！！
• X、Y、Z接AD口，正常读数即可

MPU6050使用

数据获取平台

• XS128

基本代码

• I2C读取代码

  x1=MPU6050GetData(ACCEL_XOUT_H);    //X轴输出值，待转化为角度
  y1=MPU6050GetData(ACCEL_YOUT_H);    //
  z1=MPU6050GetData(ACCEL_ZOUT_H);    //
  gyroX = (MPU6050GetData(GYRO_XOUT_H)) / 131.0;
  gyroY = (MPU6050GetData(GYRO_YOUT_H)) / 131.0;
  gyroZ = (MPU6050GetData(GYRO_ZOUT_H)) / 131.0; 
  

  • 花费时间
    
    • 1s内可以执行该段代码54次，即执行周期大约是18.51ms

• 反三角函数计算代码

  gZ=MPU6050GetAngle( x1, y1, z1,0);//把得到的角度扩大十倍
  gX=MPU6050GetAngle( x1, y1, z1,1);
  gY=MPU6050GetAngle( x1, y1, z1,2);
  

  其中函数定义如下：

  double MPU6050GetAngle(float x,float y,float z,uchar dir)
  {
      float temp;
      float res=0;
      switch(dir)
      {
          case 0://与自然Z轴的角度
              temp=sqrt((x*x+y*y))/z;
              res=atan(temp);
              break;
          case 1://与自然X轴的角度
              temp=x/sqrt((y*y+z*z));
              res=atan(temp);
              break;
          case 2://与自然Y轴的角度
              temp=y/sqrt((x*x+z*z));
              res=atan(temp);
              break;
      }
      return res*180.0/3.1415926;//把弧度转换成角度
  }
  

  • 花费时间
    
    • 1s内执行该代码段5078次，相对于I2C读取时间来说几乎可以忽略不计