直立车学习笔记（一）

  直立车是通过两个电机的前后加速度来维持车体保持一定的角度，调直立对于PID的理解有一个质的飞跃，接下来我会不定期更新我学习直立车的成果和调PID的经历，记录一下我的成果，也让其他人学习一下，避免一下我踩过的坑。

  调直立，第一点肯定是让车子立起来，立起来就需要得到车体的姿态信息，就要对车子进行姿态解算。

  智能车的姿态解算不向四轴飞行翼那样麻烦，我们只需要得到车子的俯仰角就可以用来写车子的直立环，初步让它立起来了。姿态角的解算需要用到一个姿态传感器，目前我车上用的是逐飞的Icm20602六轴姿态传感器，集成了三轴加速度计和三轴陀螺仪。

    在Icm20620中，三轴加速度计和三轴陀螺仪的三个轴是相同的。三轴陀螺仪的作用是检测角速度，角速度积分可得角度，拿哪个轴的角速度进行积分就和你的陀螺仪安装位置相关，像我这样安装的话就是拿Y轴角速度进行积分。但是陀螺仪的角速度积分会有积分误差，误差值与积分时间dt成正比。直到电路饱和，得到的角度信息就会变得不可信。三轴加速度计这个检测器件在各个方向的形变情况而采用得到受力数据，传感器输出经转换后的加速度数据。只有重力作用时，三轴加速度计输出的就是重力加速度在各个轴上的分量。通常我们需要的正切角由X轴加速度和Z轴加速度正切而来，但是三轴加速度计不会区分重力加速度和外力加速度，当物体运动时，它会在运动的方向上检测出加速度，所以加速度计对振动之类的噪声会比较敏感。所以单单用加速度计正切出姿态角，那么姿态角的波形会非常抖动，是不能用来控制直立车的。 

   那么我们该如何运用Icm20602得到可用且稳定的姿态角数据呢，其实网上有许多方法，卡尔曼滤波，四元数解算，互补滤波。我用到了其中最为简单的互补滤波，我的实践也证明互补滤波完全可以胜任直立车的控制。

  互补滤波可以看作是低通滤波和高通滤波的结合。通过以上分析可知，加速度计的静止稳定性好，但是运动时他的数据波动会变得不可靠；陀螺仪的数据稳定性好，但是容易积分产生累计误差，导致长期数据不可信。我们需要一个滤波器，使滤出的角度既有陀螺仪数据的稳定性，又有加速度数据的长期可信度。

  低通滤波器：低通滤波的目的是只让长期信号的变化量通过,过滤掉短期的波动。一种方法是力的变化建立在随后的时间一点一点地通过程序循环。比如：angle = (0.98)*angle + (0.02)*x_acc中，开始是零的角度，加速计突然跳跃到10º,角度估计的改变像随后的迭代

 如果传感器保持在10º,角度估计将会上升直至在它的水平这个值。所花费的时间,达到充分数值既取决于过滤常数(例如0.98和0.02)也取决于采样率的回路(dt)。

  高通滤波器:理论和低通滤波器相比这有点难以解释,但作用上它恰恰和低通滤波器相反:它允许短周期信号通过，过滤信号而达到稳定，这能用来抵消漂移（过滤掉直流成分）。

  采样周期:每个程序之间经过循环所需时间的长短。如果采样率是100赫兹,采样周期为0.01秒。时间常数:滤波器的时间常数表现为一个相对持续的信号，对于一个低通滤波器,分析了信号的时间常数远比通过信号改变而短于过滤的时间常数。对于高通滤波器那就是相反的，这个时间常数，τ，是一个低通滤波器。

     互补滤波器： y = (a)*(y) + (1-a)*(x);

  这表示滤波器的两个部分总是合成一部分,输出是一种精度高的值、单位的线性估计这样做是有意义的。我感觉这滤波器不是准确的互补, 但是当时间常数远比采样率大的时候是一个很好的近似(对任何数字控制来说是一个必要条件)。

  由以上分析可见，通过互补滤波，我们取合适的滤波系数，在采样频率够高的情况下，就可以得到一个兼具陀螺仪的稳定性和加速度计的可信度的数据。而Icm20602的采样频率可以高达1000Hz，所以作为结果简单，运算速度快的互补滤波成为了我姿态角数据融合的首选。

  接下来我就分享一下我互补滤波的算法

#define dtt 0.01f // 采样频率1K，采样周期0.01s
#define Cacc 0.0024f 
#define Cgyro 16.4f  

void HuBu_Filtering(RealDate_param_t *p_stRDAT，RealAngleparam_t *p_stRAG)
{

    p_stRDAT->R_acc_x =  icm_acc_x * Cacc ;                 
    p_stRDAT->R_acc_z = icm_acc_z * Cacc ;                   
    p_stRDAT->R_gyro_y = gyro_mean_filter(icm_gyro_y,8) / Cgyro ;//平滑滤波后转换单位

    p_stRAG->anglespeed = p_stRDAT->R_gyro_y;                 
    p_stRAG->angle_acc = atan2f(p_stRDAT->R_acc_z,p_stRDAT->R_acc_x) * 57.3f;          

    p_stRAG->angle_gyro = p_stRAG->angle_Filtering + p_stRAG->anglespeed * dtt;    
    p_stRAG->angle_Filtering = K1 * p_stRAG->angle_acc + (1 - K1) * p_stRAG->angle_gyro;

}

    其中Cacc是加速度转换系数，为9.8/分辨率，我的分辨率是12位，那么就是9.8/4096 = 0.0024，Cgyro是角速度转换系数，跟陀螺仪的灵敏度有关，可以参考Icm20602数据手册，我选用的是满量程2000dps，对应灵敏度为16.4，平滑滤波可要可不要。

  在调滤波参数时可以充分利用上位机，观察波形，这里推荐一个上位机叫VisualCope，比较朴实，算是好实现的。通信代码如下：

void VisualScope_Output(float data1 ,float data2 ,float data3 ,float data4)
{
  int temp[4] = {0};
  unsigned int temp1[4] = {0};
  unsigned char databuf[10] = {0};
  int i;
  unsigned short CRC16 = 0;

  temp[0] = (int)data1;
  temp[1] = (int)data2;
  temp[2] = (int)data3;
  temp[3] = (int)data4;

  temp1[0] = (unsigned int)temp[0];
  temp1[1] = (unsigned int)temp[1];
  temp1[2] = (unsigned int)temp[2];
  temp1[3] = (unsigned int)temp[3];

  for(i=0;i<4;i++)
  {
    databuf[i*2]   = (unsigned char)(temp1[i]%256);
    databuf[i*2+1] = (unsigned char)(temp1[i]/256);
  }


  CRC16 = CRC_CHECK1(databuf, 8);
  databuf[8] = CRC16%256;
  databuf[9] = CRC16/256;
  for(i=0; i<10; i++)
  {
      uart_putchar(UART_0,databuf[i]);
  }
}

void display(float ch1, float ch2, float ch3, float ch4)
{
    float OutData[4];
    OutData[0] = ch1;
    OutData[1] = ch2;
    OutData[2] = ch3;
    OutData[3] =ch4;
    VisualScope_Output(OutData[0],OutData[1] ,OutData[2] ,OutData[3]);
    //delay_ms(1000);
}

unsigned short CRC_CHECK1(unsigned char *Buf, unsigned char CRC_CNT)
{
    unsigned short CRC_Temp;
    unsigned char i,j;
    CRC_Temp = 0xffff;

    for (i=0;i<CRC_CNT; i++){
        CRC_Temp ^= Buf[i];
        for (j=0;j<8;j++) {
            if (CRC_Temp & 0x01)
                CRC_Temp = (CRC_Temp >>1 ) ^ 0xa001;
            else
                CRC_Temp = CRC_Temp >> 1;
        }
    }
    return(CRC_Temp);
}

  好的姿态角波形，波形的跟随性要强，而且要圆滑，跟随性不强会导致车的控制滞后，难以稳定，不圆滑会导致车子剧烈抖动。我的参数波形是这样的，蓝色是加速度数据，黄色是滤波脚数据，波形跟原始数据重合度高，就可以进行下一步直立环的调节了。

   关于姿态解算我就说这么多，接下去还会更新直立车的串级PID的调法和写法，大三了，学业繁忙，更新比较慢，见谅。鄙人才疏学浅，掌握的知识不多，但是有问题也可以在评论区提一下，虽然可能我也不太会，但是我一定尽力解答。