写在之前的话:

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模块，那对于整车布局就十分麻烦，所以只好用啦(别人都在用，一起跳坑)；处理完了，接下来关门放算法。

二、核心路径算法(为致敬开源，放的是原作者算法)

1 /******************** (C) COPYRIGHT 2011 ********************* ********************2 * 文件名 ：Date_analyse.c3 * 描述 ：电感数据采集与分析4 *5 * 实验平台 ：野火kinetis开发板6 * 库版本 ：7 * 嵌入系统 ：8 *9 * 作者 ：oО殇のＳo10 **********************************************************************************/

11 #include "include.h"

12 #define NM 3

13

14 int16 AD_valu[5],AD_V[5][NM],chazhi,chazhi_old;15 float AD[5],sensor_to_one[5];16 float Slope_AD_1; //用于坡道检测

17 int16 max_v[5],min_v[5]; //电感标定 采集值

18 int16 Position_transit[4]; //记录过渡点归一化的值

19 int16 AD_sum[5];20 int16 AD_MAX_NUM; //21 int16 position = 2,position_back = 1;22 floatmax_value,AD_0_max,AD_1_max,AD_2_max,AD_3_max;23

24

25 /*************************************************************************26 * 函数名称 SC_black_Init27 * 功能说明： 最大值采样28 * 参数说明：29 * 函数返回： 无30 * 修改时间：31 * 备 注：32 *************************************************************************/

33 void SC_black_Init(void)34 {35 uint16 i,j;36 int16 Position_transit_short[4];37 floatsensor_1,sensor_2,sensor_3,sensor_4;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 }63 delayms(1); //延时

64 }65 /*************** 记录的过渡点归一化 ******************/

66 sensor_1 = (float)(Position_transit_short[0] - min_v[1])/(float)(max_v[1] - min_v[1]);67 if(sensor_1 <= 0.0) sensor_1 = 0.001;68 if(sensor_1 >= 1.0) sensor_1 = 1.0;69

70 sensor_2 = (float)(Position_transit_short[1] - min_v[1])/(float)(max_v[1] - min_v[1]);71 if(sensor_2 <= 0.0) sensor_2 = 0.001;72 if(sensor_2 >= 1.0) sensor_2 = 1.0;73

74 sensor_3 = (float)(Position_transit_short[2] - min_v[4])/(float)(max_v[4] - min_v[4]);75 if(sensor_3 <= 0.0) sensor_3 = 0.001;76 if(sensor_3 >= 1.0) sensor_3 = 1.0;77

78 sensor_4 = (float)(Position_transit_short[3] - min_v[3])/(float)(max_v[3] - min_v[3]);79 if(sensor_4 <= 0.0) sensor_4 = 0.001;80 if(sensor_4 >= 1.0) sensor_4 = 1.0;81

82 Position_transit[0] = (int16)(100 *sensor_1);83 Position_transit[1] = (int16)(100 *sensor_2);84 Position_transit[2] = (int16)(100 *sensor_3);85 Position_transit[3] = (int16)(100 *sensor_4);86

87

88 flash_erase_sector(SECTOR_ADM); //擦除254扇区

89 for(i=0; i<5; i++) //电感标定的最大值写入扇区

90 {91 flash_write(SECTOR_ADM,i*4,max_v[i]);92 }93 for(i=0;i<4;i++) //过渡点归一化值写入扇区

94 {95 flash_write(SECTOR_ADM,20+i*4,Position_transit[i]);96 }97 }98 else

99 {100 for(i=0;i<3;i++)101 {102 for(j=0;j<5;j++) //读取五个电感的采样标定的最大值

103 {104 max_v[j] = flash_read(SECTOR_ADM,j*4,int16);105 }106 for(j=0;j<4;j++) //读取过渡点

107 {108 Position_transit[j] = flash_read(SECTOR_ADM,20+j*4,int16);109 }110

111 LCD_Print(29,2,"Reading");112 LCD_Print(28,4,"samples...");113 delayms(10);114 }115 }116 LCD_CLS();117 Beer_ON;118 delayms(25);119 Beer_OFF;120 }121

122 /*************************************************************************123 * 函数名称 Read_ADC124 * 功能说明： AD采集125 * 参数说明：126 * 函数返回： 无127 * 修改时间：128 * 备 注：129 *************************************************************************/

130 void Read_ADC(void)131 {132 int16 i,j,k,temp;133 int16 ad_valu[5][5],ad_valu1[5],ad_sum[5];134

135 for(i=0;i<5;i++)136 {137 ad_valu[0][i]=ad_ave(ADC1,AD9,ADC_10bit,7); //ADC0 通道

138 ad_valu[1][i]=ad_ave(ADC1,AD8,ADC_10bit,7); //ADC0 通道

139 ad_valu[2][i]=ad_ave(ADC1,AD15,ADC_10bit,7); //ADC0 通道

140 ad_valu[3][i]=ad_ave(ADC1,AD11,ADC_10bit,7); //ADC0 通道

141 ad_valu[4][i]=ad_ave(ADC1,AD13,ADC_10bit,7); //ADC0 通道

142 }143 //冒泡排序///

144 for(i=0;i<5;i++) //5个电感

145 {146 for(j=0;j<4;j++) //五个数据排序

147 {148 for(k=0;k<4-j;k++)149 {150 if(ad_valu[i][k] > ad_valu[i][k+1]) //前面的比后面的大 则进行交换

151 {152 temp = ad_valu[i][k+1];153 ad_valu[i][k+1] =ad_valu[i][k];154 ad_valu[i][k] =temp;155 }156 }157 }158 }159 for(i=0;i<5;i++) //求中间三项的和

160 {161 ad_sum[i] = ad_valu[i][1] + ad_valu[i][2] + ad_valu[i][3];162 ad_valu1[i] = ad_sum[i] / 3;163 }164 滑动平均滤波/

165 for(i = 0;i < NM-1;i ++)166 {167 AD_V[0][i] = AD_V[0][i + 1];168 AD_V[1][i] = AD_V[1][i + 1];169 AD_V[2][i] = AD_V[2][i + 1];170 AD_V[3][i] = AD_V[3][i + 1];171 AD_V[4][i] = AD_V[4][i + 1];172 }173 for(i=0;i<5;i++)174 {175 AD_V[i][NM-1] =ad_valu1[i];176 }177

178 for(i = 0;i < NM;i ++)179 {180 AD_sum[0] += AD_V[0][i];181 AD_sum[1] += AD_V[1][i];182 AD_sum[2] += AD_V[2][i];183 AD_sum[3] += AD_V[3][i];184 AD_sum[4] += AD_V[4][i];185 }186 for(i=0;i<5;i++) //求平均

187 {188 AD_valu[i] = AD_sum[i] /NM;189 AD_sum[i] = 0;190 }191 }192

193 /*************************************************************************194 * 函数名称 Date_analyse195 * 功能说明： 数据分析196 * 参数说明：197 * 函数返回： 无198 * 修改时间：199 * 备 注：200 *************************************************************************/

201 voidDate_analyse()202 {203 int16 i,max_front=0,max_back;204 static int16 max_old = 1,max_crosstalk = 1;205 static int16 position_last = 2;206 floatsensor_1;207

208 Read_ADC();209

210 /*********************归一化处理********************/

211 for(i=0;i<5;i++)212 {213 sensor_to_one[i] = (float)(AD_valu[i] - min_v[i])/(float)(max_v[i] -min_v[i]);214 if(sensor_to_one[i]<=0.0) sensor_to_one[i]=0.001;215 if(sensor_to_one[i]>1.0) sensor_to_one[i]=1.0;216

217 AD[i] = 100 * sensor_to_one[i]; //AD[i]为归一化后的值 范围为0-100

218 }219 /*******1号电感特殊归一化，用于坡道检测********/

220 sensor_1 = (float)(AD_valu[1] - min_v[1])/(float)(max_v[1] - min_v[1]);221 if(sensor_1 <= 0.0) sensor_1 = 0.001;222 Slope_AD_1 = 100 *sensor_1;223

224

225 for(i=0;i<3;i++) //找出最强的传感器

226 {227 if(AD[max_front]

232 max_back = (AD[3]>AD[4])? 3:4; //找后排最强电感

233

234 if(max_value < 43) //丢线时最大值取旧值

235 {236 max_front=max_old;237 max_value=AD[max_front];238 }239 else

240 max_old=max_front;241

242 if(abs(max_front - max_crosstalk) < 2) //防串道

243 {244 max_crosstalk =max_front;245 }246 else

247 max_front =max_crosstalk;248 AD_MAX_NUM = max_front; //传送速度控制

249

250 /****************位置解算************************/

251 if(max_front==0 && (AD[1] <= Position_transit[0] - 1)) //已经偏离0号传感器

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号传感器

266 {267 position = 3;268 }269 //~~~~~~~~~~~~~~~~~~~~~~~ 后排位置解算~~~~~~~~~~~~~~~~~~~//270 if(max_back == 3 && AD[4] <= Position_transit[2] - 1)271 {272 if(AD[4] <= Position_transit[2] - 22)273 position_back = 0;274 if(AD[4] >= Position_transit[2] - 20)275 position_back = 1;276 }277 else if(max_back == 3 && AD[4] > Position_transit[2] + 1 || max_back == 4 && AD[3] > Position_transit[3] + 1)278 {279 position_back = 2;280 }281 else if(max_back == 4 && AD[3] <= Position_transit[3] - 1)282 {283 if(AD[3] >= Position_transit[3] - 20)284 position_back = 3;285 if(AD[3] <= Position_transit[3] - 22)286 position_back = 4;287 }288

289 if(abs(position - position_last) == 2) //位置防跳变

290 position =position_last;291 position_last =position;292

293 //弯道内和导线夹角过大导致后面转向不足，此时过渡点强制增大//

294 if(position == 0 && AD_0_max < 75)295 {296 AD_0_max = 75 + abs_f(75 -AD_0_max);297 }298 else if(position == 3 && AD_2_max < 75)299 {300 AD_2_max = 75 + abs_f(75 -AD_2_max);301 }302

303 /*************计算偏移量*************/

304 if(position == 0) //左侧丢线

305 {306 chazhi = (int16)((AD[1] - abs_f(AD_0_max-AD[0]) - AD_0_max)*1.3)-25;307 }308 else if(position == 1 || position == 2) //处于中间位置

309 {310 chazhi = (int16)(AD[2] - AD[0]);311 }312 else if(position == 3) //右侧丢线

313 {314 chazhi = (int16)((abs_f(AD_2_max-AD[2]) + AD_2_max - AD[1] )*1.5)+30;315 }316 if(Stright_Flag)317 {318 chazhi = (int16)(((AD[2]-AD[0])-(AD[4]-AD[3])/10)*1.3); //长直道用斜率控制舵机

319 }320 if(UPhill_flag) //上坡时 强制用两侧电感偏移量

321 {322 chazhi = (int16)((AD[2] - AD[0]));323 }324

325 }

三、算法解读(为了不本末倒置，不再像前面一样写太多于算法之外的东西(例如过渡点存储到了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]

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之间。

后面同理，后排电感位置也能分出。

以上就轻松分出了位置，不同的位置对应了车子不同的姿态，再根据电感值差值，或者比值，就可以在不同位置给予不同系数(舵机打角控制 ，电机转速控制)，核心是根据五个值分出位置。