基于K60直立小车学习

我利用暑假空闲的时间调直立车，对陀螺仪加速度计有了了解，同时也对直立算法有了些认识。做此总结。

小车是飞思卡尔的C车模，核心板用的是K60，用陀螺仪加速度计测量角度。

想要小车直立，首先要将角度拟合正确，角度拟合的好了，才能有正确的反馈，以便更好的进行电机控制。角度拟合用陀螺仪和加速度计来测量。

先介绍下陀螺仪和加速度计：

陀螺仪：

陀螺仪可以用来测量物体的旋转角速度。陀螺仪输出的是车模的角速度，不会受到车体运动的影响，因此该信号中噪声很小。车模的角度又是通过对角速度积分获得的，这可进一步平滑信号，从而使得到的角度信息更加准确。

但是由于从陀螺仪角速度获得角度信息需要经过积分运算，如果角速度信号存在微小的偏差和漂移，经过积分运算后，会使误差积累随着时间延长逐步增加，最终导致电路饱和，无法形成正确的角度信号。

加速度计：

加速度传感器可以测量由地球引力作用或者物体运动所产生的加速度，而当加速度计发生倾斜时，其输出信号也会随倾斜角度做同比变化。只需要测出加速度就可以获得车模的倾角，再对此信号进行微分便可以获得倾角速度。

但在实际车模运行过程中，由于车模本身的摆动所产生的加速度会产生很大的干扰信号，它叠加在上述测量信号上，就会使得输出信号无法准确反映车模的倾角。

为了减少运动引起的干扰，加速度传感器安装的高度越低越好。但是无法彻底消除车模运动的影响。

总结下来就是：加速度计可以测量小车的倾角，但是加速度计在静态下测量的倾角效果好，而对于动态的小车，测量的结果会有很大的干扰，如图中黄色曲线，毛刺很明显。所以需要陀螺仪的校准，把陀螺仪和加速度计获取的数据拟合在一起，便得出准确、平滑的角度曲线。

先来看下角度控制的框图：

用于拟合角度的函数如下：

void AngleCalculate(void) 
{
  GYRO=(-1)*(Adc0_Result-C_GYRO)*R_GYRO;//Asc0_Result为陀螺仪测量值，C_GYRO为陀螺仪零偏，R_GYRO为陀螺仪比例
  ANG_Z=(Adc1_Result-C_Z)*R_Z;//Adc1_Result为加速度计测量值，R_Z为加速度计比例，C_Z为加速度计零偏
  ANG=ANG_I;
  ANG_P=(ANG_Z-ANG)/T_g;
  ANG_I+=(GYRO+ANG_P)*T_i;//T_i用于积分，ANG_P为计算出来的角度补偿量，GYRO影响曲线斜率，ANG_P影响曲线向预定值靠拢的速度。
}

函数中，各参数的确定方法，官网上《电磁组直立车模调试指南》的视频中已讲解得很详细。

角度拟合出来后，就可以进行下一步的直立控制了。

调整车模角度的控制周期很短，时间一般是几个毫秒，远小于时间常数T1。此时电机基本上运行在加速阶段。由式子“”计算得到加速度控制量a，再乘以一个比例系数，即为施加在电机上的控制电压，这样便可以控制车模保持直立状态。

直立控制使用的是PID算法，如，当测量的角大于设定的角度时，电机的控制量DTY1=((ANG - ANG_center ) * PID1_P + (GYRO-GYRO_center) * PID1_D); 接下来就是调参数了。

值得一提的是，根据官方方案，整个直立控制在中断函数中完成，这样能更精确地控制程序算法的时序。分别将AD采样、角度拟合、电机控制等模块分配到不同的5ms的片段中，如下：

void PIT0_Isr(void)
{
  PIT_CNT++;
  if(PIT_CNT==1)
  {
    Adc0_Result = adc_get_ave(ADC0, 1, 20);   //取得A0P1引脚AD值
    Adc1_Result = adc_get_ave(ADC1, 0, 20);   //取得A0P3引脚AD值
  }
  if(PIT_CNT==2)//AD采样，平均滤波
  {
    AngleCalculate();
  }
  if(PIT_CNT==3)//车模直立控制
  {
    fPWM_Control();
  }
  if(PIT_CNT>=5)
  {
    PIT_CNT=0;
  }

}

当然，让小车稳定地站立起来并非容易，影响其稳定性的因素不仅仅是PID的参数，还与小车机械结构中齿轮是否咬合的很好有关，控制周期的长短也会有影响，总之，智能车是个复杂的系统，想调好车，需要充实而全面的知识储备才行啊！