恩智浦智能车电磁组分区算法介绍

gdlgjfgj · 发表于 2016-9-8 13:30:58

恩智浦智能车分为三组：摄像头、光电、电磁，我做的是电磁车，三种车队区别在于传感器的不同，所以获得路径信息的方法也不一样，摄像头和光电识别的是赛道上的黑线（白底赛道），而电磁车则是检测埋在赛道下的通入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号传感器 63为偏离0号传感器时1号传感器的值
{
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号传感器 70为偏离3号传感器时2号传感器的值
{
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]));
}
}

复制代码

三、算法解读

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号电感值最大（不是就一定在垂直位置），(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之间。
后面同理，后排电感位置也能分出。

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

阅读全文

stary666 · 发表于 2016-9-8 14:42:59

顶一个，，

混森蓝兽 · 发表于 2016-9-8 15:18:14

智能车群里的兄弟？

唉呀波塔 · 发表于 2022-5-30 20:33:46

你好老师！我想问一下您电磁循迹是靠什么端口输出的呀

[原创] 恩智浦智能车电磁组分区算法介绍

相关帖子

站长推荐 /4