三电感位移算法.doc

这个算法是我在调试的过程中慢慢总结出来的，华东赛的结束也是我们队的结束，公布位移算法，希望能对为位移算法烦恼的同学起到一定的启示作用，也希望对其他同学有所启发，总结出更好地路径检测方案。 我的位移算法使用三个电感，如图，三个电感水平排布，总宽度为240mm。 这个算法是我去年接近十月份时才大概成型的，当时还是四个轮子，所以是以高度不变来考虑的解算方式。变成两轮直立后直接移植也发现问题不大，不过传感器高度还是不能有太大的变化，不然结果的线性度会变差。 我当时的思想大概是这样，在高度不变的时候，那么传感器的值的变化基本是确定的，随着距离的变化是一个钟形曲线，曲线的数学模型可以参考卓晴老师的文章。直接用数学模型去计算后我发现曲线拟合的结果很差，但重复测量之后数据都基本不会变化，我便把固定高度上传感器的值记录了下来，用折线近似后存在单片机内。以后每次AD读回来的值都会与这些折线比较，用线性插值的办法确定导线到该电感距离的绝对值（由于传感器一致性和赛道电流的影响，在计算之前需要对传感器进行标定，标定的方法我之后会解释）。三个电感就可以得到三个导线距离的绝对值。 接下来便是如何融合这三个距离的值。经过试验后我发现这个值在直道上比较准确，但在弯道上会有较大误差。分析采回来的数据可以看出来在弯道时如果导线在电感的接近正下方时AD值明显小于直道上的值，但在离线一定距离时AD值在同样位移下基本一样，这说明电感距离导线一定距离时的值是最可信的（当然距离也不能太远，不然测出来的值就没有参考意义了）。于是我的融合思想便是这样： 当中间传感器测出来的距离值小于50mm时，认为位移值为左边的绝对值减去右边的绝对值的差除以2； 当中间传感器测出来的距离值在70mm到110mm时，直接以中间电感测出的位移值作为位移值（这个时候位移的正负参考两边传感器的大小）； 当中间传感器测出来的距离值大于130mm时，首先判断导线在左边还是右边，在右边的话直接用120+右边电感测出得距离值作为位移值。左边的话也是一样的操作，不过算出来的位移是负的（以导线在车右边为正）。 当中间传感器测出来的距离值在以上三种算法的中间，那么根据中间传感器测出来的距离值大小进行插值计算。例如当中间传感器测出来的距离值为55时，假设按照第一种方法算出来位移是46，第二种算出来是55，那么实际位移算出来是（e-46)/(55-46)=(55-50)/(70-50)，则位移e=48.25。这样做主要是为了保证算法过渡时的连续性。 46 50 e 55 55 70 计算里面的两个55应该是代表不同的意义，我用颜色区分了，e代表计算 结果。 这样计算的一个好处是在电感的值都比较小时计算出来的位移值很大，只要方向判断没有错的话就不会出现在急弯突然位移很小的情况。 后来直立的规则出来了，那么这个算法也就不那么适用了。因为直立车模的传感器高度在不断变化，那么之前在单片机内部存的传感器数据参考意义就不那么大了，必须找到一个代替的参考值。我最后选择每次起跑前标定传感器来作为参考值（标定的结果可以用XS的D-FLASH来记录以避免每次开机都要标定）。 标定及计算的方法如下： 开机后通过拨码开关来使程序进入标定模式，标定时的动作是保持车模姿态为前进时的姿态，使传感器从左往右经过导线上方（最好保证传感器高度离高度地不变，传感器方向水平）。这个时候AD不断地读入传感器的值，我初始化了一个3*3的数组，申明为 max[3][3]，初始化全部为0。我认为max[0][0],max[1][1],max[2][2]是代表从左到右三个传感器标定出的最大值，如果测得的值大于数组中的值，则对数组中的值进行刷新。如当左边的传感器的值测出来为500，此时max[0][0]=489，那么使max[0][0]=500，max[0][1]=此时中间传感器的值，max[0][2]=此时右边传感器的值。 标定完成后得到一个3*3的数组，以max[0][0]，max[1][0]，max[2][0]为例，这三个值代表了导线在左边传感器下方，导线距离左边传感器120mm，导线距离左边传感器240mm时的值。在车辆运行的过程中进行插值计算就可以得到之前的算法中的距离绝对值。例如max[0][0]=500，max[1][0]=200，max[2][0]=40，而此次AD的值是250，则说明距离在0mm到120mm之间，插值计算： 0 500 e_abs[0] 250 120 200 则（e_abs[0]-0)/(120-0)=(250-500)/(200-500)，可以借出e_abs[0]=100； 右边的传感器可以用同样地方法计算，而中间的传感器只能计算0mm到120mm的值，不过按照之前的算法位移大于130mm时已经不考虑中间传感器的