飛思卡爾智能車電磁組分區算法介紹

寫在之前的話:

1、目前我是一名在校學生，這也是我第一次寫博客，不周之處，請多諒解；

2、此算法並非原創，借鑒自山東德州學院第八屆白楊隊（PS：個人看法，對於一些人把別人的開源東西改頭換面一下就說是自己的原創行為十分鄙視）；

3、對於此算法的理解和說明並非紙上談兵，算法已經被我運用到了小車比賽中並取得好的成績（具體就不多說了，比賽時車莫名其妙壞了，比賽前調試的速度絕對能進國賽，比較遺憾），總之這算法是我嘗試過的最好的算法；

4、這一次所介紹的只是路徑算法和一些知識普及，后面有時間會介紹其余部分算法及許多好的思路（舵機電機控制思路（不只是簡單的PID），雙車策略）；

5、希望對於這方面有涉及的人能與我聯系並交流或指出不足之處。

6、轉載請說明出處：http://www.cnblogs.com/roc-wwp/p/4848136.html（PS：看到有人直接改成恩智浦智能車，也是醉了，這是我上學時寫的，當時好自信啊，2333）

---------------------------------------------------------------分割線-----------------------------------------------------------------------------

一、沒有這方面了解的可以看看

　　飛思卡爾智能車分為三組：攝像頭、光電、電磁，我做的是電磁車，三種車隊區別在於傳感器的不同，所以獲得路徑信息的方法也不一樣，攝像頭和光電識別的是賽道上的黑線（白底賽道），而電磁車則是檢測埋在賽道下的通入100mh電流的漆包線，攝像頭和光電采用的是攝像頭和ccd作為傳感器，電磁則是用電感放在漆包線周圍，則電感上就會產生感應電動勢，且感應電動勢的大小於通過線圈回路的磁通量成正比，又因為漆包線周圍的磁感應強度不同，因此不同位置的電感的感應電動勢就不同，因此就可以去確定電感位置；因此在車子前面設置了50cm的前瞻，電感布局如下(怎么發不了圖片)：分為兩排，前排3個，編號0，1，2（前期還加了兩個豎直電感用來幫助過直角彎，后來改為了八字電感）；后排2個，編號3，4；現在車子獲得了不同位置的感應電動勢的大小了，但這些值是不能處理的:1、感應電動勢太微弱；2、是模擬信號，信號太微弱就放大它；這就涉及到模擬電路的知識了，就不多說了（因為要把這講完到PCB繪制的篇幅就足夠寫另開一號專門寫這些方面來（PS：題外話（我的題外話比較多））：放大部分外圍你設計的再好也抵不過一個更好的芯片，有兩個例子，一個是我自己的:之前用的是NE5532，但是效果不理想，加了好多什么濾波，補償，都用上，沒用，軟件里處理后面再說，后來一狠心換了AD620，感覺像是春天來了，因為它是儀用放大器，還有就是貴。。。，效果是超級贊，去掉了許多附加電路，板子簡潔多了，（器件多，故障率也會提高，到后期查故障簡直是惡心（能想象拿着萬用表和示波器在實驗室糾結的樣子嗎？感覺怎樣處理故障還可以專門總結一下））另一個是學長的例子：他參加電子設計競賽時，有一組（同一學校的）和他題目一樣，東西做出來后基本差不多，但是他拿了一等獎，而另一組是二等獎，差別就在於當時的一個濾波，本校指導老師說可以加一個簡單電容就行，而他用的則是一款新的濾波芯片，信號最后出來的波形差距挺大的），（我負責的主要是軟件部分，但是硬件電路也全程涉及了，不過終究還是不太熟）。

　　解決完放大就是把模擬信號轉換為數字信號了，我們直接用的是芯片上的AD模塊，（題外話又來啦：我開始用的是K60是飛思卡爾Kinetis微控制器ARM Cortex®-M4系列，其實就是把它當成了一款高性能的單片機了，實在是浪費，（現在在學嵌入式開發，希望有高手來指導我），后來改為s12了，因為與別的組沖突，感覺K60確實比s12更容易上手，s12還要自己配置寄存器，級聯，計算超頻，外部時鍾頻率，k60別人寫好了庫，就是在不斷調用），不過看論壇上有人說飛思卡爾的AD轉換有問題，而飛思卡爾也不公開它AD模塊具體細節（這貌似有點心虛了，呵呵），不過要是自己做ad模塊，那對於整車布局就十分麻煩，所以只好用啦（別人都在用，一起跳坑）；處理完了，接下來關門放算法。

二、核心路徑算法（為致敬開源，放的是原作者算法）

 

/******************** (C) COPYRIGHT 2011 ********************* ********************
 * 文件名       ：Date_analyse.c
 * 描述         ：電感數據采集與分析
 *
 * 實驗平台     ：野火kinetis開發板
 * 庫版本       ：
 * 嵌入系統     ：
 *
 * 作者         ：oО殤のＳo 
**********************************************************************************/    
#include "include.h"
#define   NM    3

int16  AD_valu[5],AD_V[5][NM],chazhi,chazhi_old;
float  AD[5],sensor_to_one[5];
float  Slope_AD_1;  // 用於坡道檢測
int16  max_v[5],min_v[5];  //電感標定 采集值
int16  Position_transit[4];  //記錄過渡點歸一化的值
int16  AD_sum[5]; 
int16  AD_MAX_NUM;   //
int16  position = 2,position_back = 1;
float  max_value,AD_0_max,AD_1_max,AD_2_max,AD_3_max;


/*************************************************************************
*  函數名稱   SC_black_Init
*  功能說明： 最大值采樣
*  參數說明：         
*  函數返回： 無
*  修改時間：
*  備    注：
*************************************************************************/
void SC_black_Init(void)
{
   uint16  i,j;
   int16   Position_transit_short[4];
   float   sensor_1,sensor_2,sensor_3,sensor_4;
   if(K2)
   {
       LCD_Print(25,2,"Collecting"); 
       LCD_Print(28,4,"samples..."); 
   
       max_v[0] = max_v[1] = max_v[2] = max_v[3] = max_v[4] = 0;
       min_v[0] = min_v[1] = min_v[2] = min_v[3] = min_v[4] = 7;  
       for(i=0;i<1200;i++) 
       {
           AD_valu[0] = ad_ave(ADC1,AD9,ADC_10bit,6);  //PTC0  通道      
           AD_valu[1] = ad_ave(ADC1,AD8,ADC_10bit,6);  //PTC1  通道     
           AD_valu[2] = ad_ave(ADC1,AD15,ADC_10bit,6); //PTE25 通道     
           AD_valu[3] = ad_ave(ADC1,AD11,ADC_10bit,6); //PTE24 通道 
           AD_valu[4] = ad_ave(ADC1,AD13,ADC_10bit,6); //PTE24 通道 
           for(j=0;j<5;j++) 
           {      
               if(AD_valu[j] > max_v[j]) 
               {
                   max_v[j] = AD_valu[j];
                   if(j==0)    Position_transit_short[0] =  AD_valu[1];  //記錄過渡點 電感值
                   if(j==2)    Position_transit_short[1] =  AD_valu[1];  
                   if(j==3)    Position_transit_short[2] =  AD_valu[4];  
                   if(j==4)    Position_transit_short[3] =  AD_valu[3];  
               }
           }
           delayms(1);           //延時    
       }
       /***************              記錄的過渡點歸一化                     ******************/
       sensor_1 = (float)(Position_transit_short[0] - min_v[1])/(float)(max_v[1] - min_v[1]); 
       if(sensor_1 <= 0.0)  sensor_1 = 0.001;
       if(sensor_1 >= 1.0)  sensor_1 = 1.0; 
       
       sensor_2 = (float)(Position_transit_short[1] - min_v[1])/(float)(max_v[1] - min_v[1]); 
       if(sensor_2 <= 0.0)  sensor_2 = 0.001;
       if(sensor_2 >= 1.0)  sensor_2 = 1.0; 
       
       sensor_3 = (float)(Position_transit_short[2] - min_v[4])/(float)(max_v[4] - min_v[4]); 
       if(sensor_3 <= 0.0)  sensor_3 = 0.001;
       if(sensor_3 >= 1.0)  sensor_3 = 1.0; 
       
       sensor_4 = (float)(Position_transit_short[3] - min_v[3])/(float)(max_v[3] - min_v[3]); 
       if(sensor_4 <= 0.0)  sensor_4 = 0.001;
       if(sensor_4 >= 1.0)  sensor_4 = 1.0; 
       
       Position_transit[0] = (int16)(100 * sensor_1);
       Position_transit[1] = (int16)(100 * sensor_2); 
       Position_transit[2] = (int16)(100 * sensor_3);
       Position_transit[3] = (int16)(100 * sensor_4); 
       
       
       flash_erase_sector(SECTOR_ADM);       //擦除254扇區    
       for(i=0; i<5; i++)                    //電感標定的最大值寫入扇區
       { 
           flash_write(SECTOR_ADM,i*4,max_v[i]);     
       }  
       for(i=0;i<4;i++)                     //過渡點歸一化值寫入扇區 
       {
          flash_write(SECTOR_ADM,20+i*4,Position_transit[i]);  
       }
   }
   else 
   {
       for(i=0;i<3;i++)
       { 
          for(j=0;j<5;j++)   //讀取五個電感的采樣標定的最大值
          {
              max_v[j] = flash_read(SECTOR_ADM,j*4,int16);
          }
          for(j=0;j<4;j++)  //讀取過渡點
          {
             Position_transit[j] = flash_read(SECTOR_ADM,20+j*4,int16);
          }
            
          LCD_Print(29,2,"Reading"); 
          LCD_Print(28,4,"samples..."); 
          delayms(10);           
       }
   }
   LCD_CLS();             
   Beer_ON; 
   delayms(25);  
   Beer_OFF;
}

/*************************************************************************
*  函數名稱   Read_ADC
*  功能說明： AD采集
*  參數說明：         
*  函數返回： 無
*  修改時間：
*  備    注：
*************************************************************************/
void Read_ADC(void)
{
     int16  i,j,k,temp;
     int16  ad_valu[5][5],ad_valu1[5],ad_sum[5];
     
     for(i=0;i<5;i++)
     {
         ad_valu[0][i]=ad_ave(ADC1,AD9,ADC_10bit,7);     // ADC0 通道 
         ad_valu[1][i]=ad_ave(ADC1,AD8,ADC_10bit,7);     // ADC0 通道 
         ad_valu[2][i]=ad_ave(ADC1,AD15,ADC_10bit,7);     // ADC0 通道
         ad_valu[3][i]=ad_ave(ADC1,AD11,ADC_10bit,7);     // ADC0 通道 
         ad_valu[4][i]=ad_ave(ADC1,AD13,ADC_10bit,7);     // ADC0 通道 
     }
  //////////////////////冒泡排序///////////////////////////////////
     for(i=0;i<5;i++)     //5個電感     
     {
        for(j=0;j<4;j++)  //五個數據排序
        {
           for(k=0;k<4-j;k++)
           {
              if(ad_valu[i][k] > ad_valu[i][k+1])  //前面的比后面的大  則進行交換
              {
                 temp = ad_valu[i][k+1];
                 ad_valu[i][k+1] = ad_valu[i][k];
                 ad_valu[i][k] = temp;
              }
           }
        }
     }
     for(i=0;i<5;i++)    //求中間三項的和
     {
        ad_sum[i] = ad_valu[i][1] + ad_valu[i][2] + ad_valu[i][3];       
        ad_valu1[i] = ad_sum[i] / 3;
     }
  ////////////////////////滑動平均濾波/////////////////////////////
     for(i = 0;i < NM-1;i ++)
     {
         AD_V[0][i] = AD_V[0][i + 1];
         AD_V[1][i] = AD_V[1][i + 1];
         AD_V[2][i] = AD_V[2][i + 1];
         AD_V[3][i] = AD_V[3][i + 1];
         AD_V[4][i] = AD_V[4][i + 1];
     }
     for(i=0;i<5;i++)
     {
         AD_V[i][NM-1] =  ad_valu1[i];
     }
    
     for(i = 0;i < NM;i ++)
     {
         AD_sum[0] += AD_V[0][i];
         AD_sum[1] += AD_V[1][i];
         AD_sum[2] += AD_V[2][i];
         AD_sum[3] += AD_V[3][i];
         AD_sum[4] += AD_V[4][i];
     }
     for(i=0;i<5;i++)  //求平均
     {
         AD_valu[i] = AD_sum[i] / NM;
         AD_sum[i] = 0;   
     }    
}

/*************************************************************************
*  函數名稱   Date_analyse
*  功能說明： 數據分析
*  參數說明：         
*  函數返回： 無
*  修改時間：
*  備    注：
*************************************************************************/
void Date_analyse()
{ 
    int16  i,max_front=0,max_back; 
    static int16 max_old = 1,max_crosstalk = 1;
    static int16 position_last = 2;
    float  sensor_1;
    
    Read_ADC();
    
    /*********************歸一化處理********************/
    for(i=0;i<5;i++) 
    {
       sensor_to_one[i] = (float)(AD_valu[i] - min_v[i])/(float)(max_v[i] - min_v[i]); 
       if(sensor_to_one[i]<=0.0)  sensor_to_one[i]=0.001;
       if(sensor_to_one[i]>1.0)  sensor_to_one[i]=1.0; 
       
       AD[i] = 100 * sensor_to_one[i];     //AD[i]為歸一化后的值  范圍為0-100
    }      
       /*******1號電感特殊歸一化，用於坡道檢測********/
     sensor_1 = (float)(AD_valu[1] - min_v[1])/(float)(max_v[1] - min_v[1]);  
     if(sensor_1 <= 0.0)  sensor_1 = 0.001;
     Slope_AD_1 = 100 * sensor_1;
  ////////////////////////////////////////////////////////////////////
    
      for(i=0;i<3;i++)                 //找出最強的傳感器
      {  
        if(AD[max_front]<AD[i]-2)
          max_front=i;
      } 
      max_value=AD[max_front]; 
      
      max_back = (AD[3]>AD[4])? 3:4;  //找后排最強電感
      
      if(max_value < 43)            //丟線時最大值取舊值 
      {
        max_front=max_old;
        max_value=AD[max_front]; 
      }
      else
        max_old=max_front;
      
      if(abs(max_front - max_crosstalk) < 2)  //防串道
      {
           max_crosstalk = max_front;
      }
      else 
           max_front = max_crosstalk;
      AD_MAX_NUM = max_front;        //傳送速度控制
      
      /****************位置解算************************/
      if(max_front==0 && (AD[1] <= Position_transit[0] - 1))  //已經偏離0號傳感器   
      {
            position=0;  
      }   
      else if((max_front==0 && (AD[1] > Position_transit[0] + 1)) || (max_front==1 && (AD[0] - AD[2]) > 1))  //左側位置  0-1號傳感器之間
      {    
            position=1; 
            AD_0_max = AD[0];  //記錄下此時的3號傳感器的值
      }
      else if((max_front==1 && (AD[2] - AD[0]) > 1) || (max_front==2 && (AD[1] > Position_transit[1] + 1)))  //右側位置  1-2號傳感器之間
      {
            position=2;
            AD_2_max = AD[2];  //記錄下此時的3號傳感器的值
      }
      else if(max_front==2 && (AD[1] <= Position_transit[1] - 1))  //已經偏離3號傳感器    
      {        
            position = 3;  
      }  
      //~~~~~~~~~~~~~~~~~~~~~~~ 后排位置解算~~~~~~~~~~~~~~~~~~~//
      if(max_back == 3 && AD[4] <= Position_transit[2] - 1)
      { 
           if(AD[4] <= Position_transit[2] - 22)
              position_back = 0;
           if(AD[4] >= Position_transit[2] - 20)
              position_back = 1;
      }
      else  if(max_back == 3 && AD[4] > Position_transit[2] + 1 || max_back == 4 && AD[3] > Position_transit[3] + 1)
      {
           position_back = 2;
      }
      else if(max_back == 4 && AD[3] <= Position_transit[3] - 1)
      {
           if(AD[3] >= Position_transit[3] - 20)
              position_back = 3;
           if(AD[3] <= Position_transit[3] - 22)
              position_back = 4;
      }
      
      if(abs(position - position_last) == 2)  //位置防跳變
           position = position_last;
      position_last = position;
      
     //////彎道內和導線夾角過大導致后面轉向不足，此時過渡點強制增大//////////////  
      if(position == 0 && AD_0_max < 75)
      {
          AD_0_max = 75 + abs_f(75 - AD_0_max);    
      }
      else if(position == 3 && AD_2_max < 75)
      {
          AD_2_max = 75 + abs_f(75 - AD_2_max); 
      }
      
      /*************計算偏移量*************/
      if(position == 0)   //左側丟線  
      {
           chazhi = (int16)((AD[1] - abs_f(AD_0_max-AD[0]) - AD_0_max)*1.3)-25;
      }
      else if(position == 1 || position == 2) //處於中間位置
      {
               chazhi = (int16)(AD[2] - AD[0]);           
      }
      else if(position == 3)  //右側丟線
      {
           chazhi = (int16)((abs_f(AD_2_max-AD[2]) + AD_2_max - AD[1] )*1.5)+30;
      }
      if(Stright_Flag)
      {
          chazhi = (int16)(((AD[2]-AD[0])-(AD[4]-AD[3])/10)*1.3);  //長直道用斜率控制舵機
      }
      if(UPhill_flag)  //上坡時 強制用兩側電感偏移量
      {
          chazhi = (int16)((AD[2] - AD[0])); 
      }
      
}

 

 三、算法解讀（為了不本末倒置，不再像前面一樣寫太多於算法之外的東西（例如過渡點存儲到了flash能掉電保存），軟件濾波——關於這些我有機會再開一篇慢慢說）

路徑分區核心在這：

 38    if(K2) 
 39  { 40 LCD_Print(25,2,"Collecting"); 41 LCD_Print(28,4,"samples..."); 42 43 max_v[0] = max_v[1] = max_v[2] = max_v[3] = max_v[4] = 0; 44 min_v[0] = min_v[1] = min_v[2] = min_v[3] = min_v[4] = 7; 45 for(i=0;i<1200;i++) 46  { 47 AD_valu[0] = ad_ave(ADC1,AD9,ADC_10bit,6); //PTC0 通道 48 AD_valu[1] = ad_ave(ADC1,AD8,ADC_10bit,6); //PTC1 通道 49 AD_valu[2] = ad_ave(ADC1,AD15,ADC_10bit,6); //PTE25 通道 50 AD_valu[3] = ad_ave(ADC1,AD11,ADC_10bit,6); //PTE24 通道 51 AD_valu[4] = ad_ave(ADC1,AD13,ADC_10bit,6); //PTE24 通道 52 for(j=0;j<5;j++) 53  { 54 if(AD_valu[j] > max_v[j]) 55  { 56 max_v[j] = AD_valu[j]; 57 if(j==0) Position_transit_short[0] = AD_valu[1]; //記錄過渡點 電感值 58 if(j==2) Position_transit_short[1] = AD_valu[1]; 59 if(j==3) Position_transit_short[2] = AD_valu[4]; 60 if(j==4) Position_transit_short[3] = AD_valu[3]; 61  } 62  }
能傳圖片了，原來是我看錯了格式。。。

沒有這個圖還真不好講，了解之前最好要有一定物理認知，圖中藍線為電磁線（實際只有一條，我畫了三種位置），1、電磁線與電感（圖中紅色）垂直有最大感應電動勢，平行時最小；2、賽車采集電感值時圖中兩條黑線（前瞻）左右最大賽道范圍擺動，即一定會得到垂直位置，就能得到最大值。
PS：K2是一個按鍵，即一個io口
檢測到k2閉合，（配置初始低電平（下拉），K2按下，接到高電位，檢測為高電平1，開始執行）

 40        LCD_Print(25,2,"Collecting"); 
 41        LCD_Print(28,4,"samples..."); 
在OLED顯示，提示作用，（25，2）：坐標

 45        for(i=0;i<1200;i++) 

采集執行的時間

47            AD_valu[0] = ad_ave(ADC1,AD9,ADC_10bit,6);  //PTC0  通道      
48            AD_valu[1] = ad_ave(ADC1,AD8,ADC_10bit,6);  //PTC1  通道     
49            AD_valu[2] = ad_ave(ADC1,AD15,ADC_10bit,6); //PTE25 通道     
50            AD_valu[3] = ad_ave(ADC1,AD11,ADC_10bit,6); //PTE24 通道 
51            AD_valu[4] = ad_ave(ADC1,AD13,ADC_10bit,6); //PTE24 通道 
五個電感采集數值放大后送入AD口，

ad_ave(ADC1,AD9,ADC_10bit,6) 采集6次取均值

重要部分：

 52            for(j=0;j<5;j++) 
 53            {      
 54                if(AD_valu[j] > max_v[j]) 
 55                {
 56                    max_v[j] = AD_valu[j];
 57                    if(j==0)    Position_transit_short[0] =  AD_valu[1];  //記錄過渡點 電感值
 58                    if(j==2)    Position_transit_short[1] =  AD_valu[1];  
 59                    if(j==3)    Position_transit_short[2] =  AD_valu[4];  
 60                    if(j==4)    Position_transit_short[3] =  AD_valu[3];  
 61                }
 62            }

理解如下：1200次的時間內電感有時間擺動幾個周期，即得到了最大值，比如在上面第一條藍色線位置，此時得到的AD_valu[0]賦給max_v[0]，注意這句if(j==0)    Position_transit_short[0] =  AD_valu[1];它把0號電感最大時1號電感的值 AD_valu[1]賦給了Position_transit_short[0]；稱Position_transit_short[0]為過渡點，后面同理，記下來當某個電感值最大時，另一電感的值。
下面跳到

225       for(i=0;i<3;i++)                 //找出最強的傳感器
226       {  
227         if(AD[max_front]<AD[i]-2) 228 max_front=i; 229  } 230 max_value=AD[max_front]; 231 232 max_back = (AD[3]>AD[4])? 3:4; //找后排最強電感

這提出來目的是為下面服務，以免不知到為什么跳出來最強電感值（這找最大值就不用說了，都能看懂）

又是一關鍵部分：

250       /****************位置解算************************/
251       if(max_front==0 && (AD[1] <= Position_transit[0] - 1))  //已經偏離0號傳感器   63為偏離0號傳感器時1號傳感器的值
252       {
253             position=0; 254  } 255 else if((max_front==0 && (AD[1] > Position_transit[0] + 1)) || (max_front==1 && (AD[0] - AD[2]) > 1)) //左側位置 0-1號傳感器之間 256  { 257 position=1; 258 AD_0_max = AD[0]; //記錄下此時的3號傳感器的值 259  } 260 else if((max_front==1 && (AD[2] - AD[0]) > 1) || (max_front==2 && (AD[1] > Position_transit[1] + 1))) //右側位置 1-2號傳感器之間 261  { 262 position=2; 263 AD_2_max = AD[2]; //記錄下此時的3號傳感器的值 264  } 265 else if(max_front==2 && (AD[1] <= Position_transit[1] - 1)) //已經偏離3號傳感器 70為偏離3號傳感器時2號傳感器的值 266  { 267 position = 3; 268  } 

position是區域，例如，0，1，2三個電感划出來了4個區域，分別為

position=0；

position=1；

position=2；

position=3；
對應圖中4個區域

if(max_front==0 && (AD[1] <= Position_transit[0] - 1))

max_front==0說前排3個電感此時0號電感值最大（不是就一定在垂直位置[垂直位置是指0號電感所有值中最大的值的位置]），(AD[1] <= Position_transit[0] - 1)說1號電感的值小於過渡點的值，說明1號電感與漆包線位置距離大於過渡點時距離，漆包線位置只能在0號電感左面（最好自己拿一輛車按這個電感布局試一下，看看值的變化），記為

position=0；

else if((max_front==0 && (AD[1] > Position_transit[0] + 1)) || (max_front==1 && (AD[0] - AD[2]) > 1))
同理分析(max_front==0 && (AD[1] > Position_transit[0] + 1))

(max_front==1 && (AD[0] - AD[2]) > 1加上了這個（我自己用的時候沒用這個）理解為上了個保險，這個更好理解，1號電感最大時，0號電感值又大於2號，漆包線位置只能在0～1之間。
后面同理，后排電感位置也能分出。

以上就輕松分出了位置，不同的位置對應了車子不同的姿態，再根據電感值差值，或者比值，就可以在不同位置給予不同系數（舵機打角控制 ，電機轉速控制），核心是根據五個值分出位置。