前言

  今天心血来潮，突然想总结一下电磁车的一些基本算法，也希望可以给新手同学们一点帮助和参考。 电磁车的控制相对比较简单，大致可以分为信号采集，舵机控制和电机控制三部分，所谓电磁车控制算法，其实就是对这三个模块功能的研究和精确，从而使赛车达到我们想要的效果。    

采样信号滤波

什么是滤波

  滤波（Wave filtering）是将信号中特定波段频率滤除的操作，是抑制和防止干扰的一项重要措施。它可以从含有干扰的接收信号中提取有用信号，很大程度上保证了采集到的信号的真实性和稳定性。正因如此，滤波算法也成了在智能车控制中不可或缺的一种控制算法。  

常见的软件滤波办法

  软件滤波在嵌入式的数据采集和处理中有着很重要的作用。对于电磁车来说，我们需要对采集到的赛道电磁信号进行滤波处理，这里我们介绍几种常见的软件滤波处理办法。  

限幅法

  一般来说，每个电感在赛道上采集到的值都有一个固定的范围，由于电感排布方案的不同（水平电感、竖直电感或斜电感等）或不同赛道电磁信号的差别，这个范围也会随之变化。这时我们可以在程序的初始化中加入一个扫描赛道的程序，在初始化程序运行时令车左右扫描，将每个电感的最大值和最小值分别记录入两个一维数组中，让其作为每个电感采集信号值的上下限（这样也是为了提高赛车对于不同赛道的适应性）。在后续赛车行驶过程中，便可以过滤在这个范围之外的电感值，从而排除偶然误差，达到软件滤波的效果。 具体代码如下（此处只写一个其中电感的处理）：  

// 扫描赛道 //
void saomiao()
{   
     uint32 i;
     uint16 max1=0,min1=4095;
        
     for(i=0;i<1000;i++)  
     {       
       LeftADC[0]=ADC_Ave(ADC0,ADC1_SE8,ADC_12bit,10); 
       if(LeftADC[0]>max1) 
	   	 max1=LeftADC[0];
       if(LeftADC[0]<min1)
      	 min1=LeftADC[0];
     }
     
} 


// 限幅法滤波 //
void AD_value()
{    
    LeftADC[0]=ADC_Ave(ADC0,ADC1_SE8,ADC_12bit,10); 
    if(LeftADC[0]>max1) 
	  	LeftADC[0]=max1;
    if(LeftADC[0]<min1)
        LeftADC[0]=min1;
        
}

算术平均值滤波

  算术平均值滤波，指的是将每个电感采集到的一组值用冒泡排序按从大到小或者从小到大的顺序排布，再丢弃最大值与最小值，取剩下数据的算术平均数，作为反馈给单片机的实时数据。这也是最常见的软件滤波算法之一。具体代码如下：  

void Ad_Value()     
{  
   uint16  LeftADC[7];
   int i,j,t;
   for(i=0;i<7;i++)
    {   
    	LeftADC[i]= ADC_Ave(ADC0,ADC1_SE8 ,ADC_12bit,10); 
    }
    
    for(i=0;j<7;i++)    
    {
      if(LeftADC[j]>LeftADC[j+1])
      {
        t=LeftADC[j+1];
        LeftADC[j+1]=LeftADC[j];
        LeftADC[j]=t;
      }
    }
    
    LeftAverage[0]=(uint16)((LeftADC[1]+LeftADC[2]+LeftADC[3]+LeftADC[4]+LeftADC[5])/5.0);   
    
}

归一化

什么是归一化

  归一化就是将所有数据都变成0-1之间的数，将数据映射到0～1范围之内处理，使数据观察更便捷快速。在电磁车行驶过程中，由于需要通过分析各个电感采集值的情况来判定前方为何种路段，所以更要求这些采集值有迹可循，归一化便是一种很好的方法，先将所有采集到的值缩小到0-1范围内，再适当放大（常见的放大倍数为100），这样既能更容易的由电磁值分析出路况，又不会因为数据过小而失真。  

归一化的代码实现

  归一化的公式如下：(x-Min)/（Max-Min）。 其中，x为实时检测到的变量，Min与Max为标定的电感采集最小与最大值。 具体代码如下：  

    AD_M_Left[0] =(uint16)(99*(LeftAverage[0]-M_Left_min)/(M_Left_max[0]-M_Left_min)+1);

PID控制

  在介绍PID算法前，我们可以试想一下，如果没有某些特殊的算法，那么我们会如何控制舵机打角和电机转速？就舵机而言，其结论可能是，设定一个特定的值，当左右电感的电磁值之差达到这个设定的值时，便控制舵机向左或向右打一定的角度（或者多设值，分不同情况多段打角），实际上这也是我在刚接触智能车时用的算法，这种算法虽然能够让赛车在赛道上行驶，但打角不够顺滑，反应也不够灵敏，这时我们便需要将这个差值代入某个算法，令其与舵机打角关联起来，这样便可以得到一个较为连贯与精确的舵机输出值，有利于赛车更完美的运行，而这种将差值与最后输出值关联起来的算法便是我们常说的PID控制算法。  

什么是PID控制

  比例（P）积分（I）微分（D）控制（PID控制），是应用最为广泛的一种自动控制器。它具有原理简单，易于实现，适用面广，控制参数相互独立，参数的选定比较简单等优点。PID控制分为很多种，而智能车中常用增量式和位置式两种算法对车身进行控制，以保证赛车在赛道上完美运行。  

PID算法的代码实现

舵机控制

  在对舵机的控制中，我采用的是位置式PD算法。将左右电感的偏差值赋给error，再代入公式进行解算，最后传给舵机输出子函数，控制舵机正确打角。  

void steer_control()  
{
    error=AD_M_Left[0]-AD_M_Right[0];
    
    direction_controlout=dirP*error+dirD*(error-lasterror);
    angle=mid_angle+direction_controlout;
    
    angle=angle>right_angle?right_angle:angle;
    angle=angle<left_angle?left_angle:angle;
    
    PWM_SetSteer((int)angle); 
    lasterror=error;
    
}

  另外，如果想使舵机更顺滑，可以对偏差error的计算进行处理，如将归一化后的值开方处理等。  

电机控制

  在对电机的控制中，我采用的是增量式PID算法，首先要设定一个目标速度set_speed，再将当前速度与其比较，得出的差值代入公式计算，得到最终需要输出的电机占空比，从而控制电机转速。需要注意的是，电机PID控制作为闭环控制，最好将其放入PIT定时器中断，同时在中断中用左右两个编码器分别测出实时速度，这样可以使其控制的更精确。  

void PID_computer_newL(int left_speed_in)
{
    ec_left=set_speed-left_speed_in;
    
    pwm_L+=speedL_P*(ec_left)+speedL_I*(ec_left-eb_left)+speedL_D*(ec_left-2*eb_left+ea_left);
    if(pwm_L> 600)  pwm_L=600;  
    if(pwm_L<-600)  pwm_L=-600; 
    
    PWM_SetMotorL((int)pwm_L);  
    
    ea_left=eb_left;
    eb_left=ec_left;
    
}

目标速度的确定

  上面我们说了PID算法可以依据反馈的差值，与最后想要的变量关联起来，达到动态控制的效果，那么为什么不能设置同一个目标速度，让赛车以同一速度跑完全程？为什么还要设置不同的目标速度？我们知道由于赛车自身机械性能的限制，在不同赛道元素中速度的上限不同，为了使赛车可以更快地跑完全程，我们需要根据赛道元素的不同设置更适合当前路段的目标速度，而对于电磁车来说，可以感知赛道的只有电磁传感器，所以我们只能将左右电感的差值关联目标速度，在差值小的时候认为赛车处于直道，此时设置一个最大速度，再通过PID算法将电机转速保持在这个水准；若差值偏大，则调小目标速度，从而达到控制目标速度动态变化的目的。   而在调试过程中我们可能会发现，仅仅靠舵机打角并不能很好的使赛车精确转弯，这时候我们就需要对两个电机的目标速度进行分别控制，通过左右电机的差速辅助赛车转弯。