写在前面

我将我的参赛代码已经在GitHub上进行了开源，欢迎大家下载
2020年全国大学生智能汽车竞赛浙江赛区二等奖开源代码

接下来我将就十六届智能车竞赛为各位同学分享我的感知赛道普通元素的思路，并且手把手教大家如何将我的开源代码加以利用，关于我的代码有什么不明白的都可以在评论区评论，博主看到了会第一时间回复的。

不过大家一定要注意，2021年的主控芯片跟2020年的不一样的，我用的主控芯片MK66FN2VLQ18，然后今年要求的芯片是 Infineon系列的单片机了。我的代码工程是用IAR8.25及以上版本才能正常打开，所以我推荐大家拿我的代码做为一个参考即可。

用到的一些算法

1.卡尔曼滤波

2.非库函数的开平方算法，贼好用，其中，0x5f3759df又被称为“神的数字”

 float my_sqrt(float number)
{
    long i;
    float x, y;
    const float f = 1.5F;
    x = number * 0.5F;
    y = number;
    i = * ( long * ) &y;
    i = 0x5f3759df - ( i >> 1 );

    y = * ( float * ) &i;
    y = y * ( f - ( x * y * y ) );
    y = y * ( f - ( x * y * y ) );
    return number * y;

感知代码

我采用的电感摆放方式是这样的,5个电感均采用横电感的摆放方式

下面是我数据感知的流程图

数据感知的基本流程图如上，接下来我主要分享的是我处理差比和数据的方法，也是我稳定车子姿态的重要一环
这一段代码是稳定运行姿态的关键，根据控制的原理，偏差大是注重偏差变化，偏差小是注重偏差的变化率，我这里做了硬件的体现。我前面三个电感用于感知远方的信息，使车子能看得更远，而后面2个电感用于帮我稳定车子因为长前瞻导致过弯的时候差比和数值很小导致过弯打角不够的问题，所以我比较前面3个电感差比和和后面2个电感差比和的大小。哪个数值大取哪个，解决打角不足的问题，效果显著~

至于卡尔曼滤波函数我就直接使用的是GitHub里的开源工程里面的函数(如有侵权，请在下方评论指出，立刻删除)，参数啥的都没调过，效果还不错，推荐直接使用即可

处理代码

差比和

我用的算法是开根号差比和，可以很大程度上增加电感信号的变化的线性度，差比和之后进行卡尔曼滤波
前面3个电感差比和

后面2个电感差比和

//水平电感差比和
  curr_elect_val =  (my_sqrt(elect_R) - my_sqrt(elect_L ))/ (my_sqrt(elect_L) + my_sqrt(elect_R)+ my_sqrt(elect_M));
  //竖直电感差比和
  up_curr_elect_val =(my_sqrt(elect_Up_R ) - my_sqrt(elect_Up_L )) / (my_sqrt(elect_Up_R ) + my_sqrt(elect_Up_L) + my_sqrt(elect_M));

  curr_elect_val = KalmanFilter_Elect(curr_elect_val,last_elect_val);                 //卡尔曼滤波
  up_curr_elect_val = KalmanFilter_Elect(up_curr_elect_val,up_last_elect_val);

这一部分是车子姿态优化的处理，很好用~
思路就是计算出前面三个电感的差比和数值和后面2个电感数值的差比和数值，然后进行比较，如果后面2个电感差比和数值大的话就采用后面2个电感的差比和数值，因为在过弯的时候前面3个电感因为前瞻太长了，伸出赛道太多了导致差比和数值太小，打角不足而冲出赛道，所以我加了后面2个电感用于在过弯的时候弥补打角不足的问题，因为后面2个电感离车子比较近，所以在过弯的时候差比和数值还是会比较大的哦 这正是“大偏差注重偏差，小偏差注重偏差变化率”的硬件体现哦。

  if(fabs(curr_elect_val) <= fabs(up_curr_elect_val)){//近端电感差比和大，弯道中间
    curr_elect_val = up_curr_elect_val;
    elect_val_delta = up_elect_val_delta;
  }

丢线

这一部分很重要!!!!!!!
这一部分很重要!!!!!!!
这一部分很重要!!!!!!!
车子跑起来总会因为一些原因偏离赛道中心线，当小车的前瞻离赛道中心很远之后，电感感知到的数值就会变得很小了，这个时候车子的姿态就已经很不好了，所以得赶紧纠正回来。
我在电磁处理程序的部分里进行电磁数值的判断，如果小于电磁阈值之后就判断会丢值，就进入丢值处理(以下称为丢线)。我的处理是这样的，判断丢值的时候哪边的电感数值大，就往数值大那的那边打死角，一直打到电感数值回到正常水平为止，以纠正姿态。

丢线处理函数的调用

丢线处理