T-ST闭环循迹算法

这是一个使用时间的光电闭环循迹算法

前导

众所周知,大部分的巡线算法都需要编码器来构成闭环系统

那要是没有编码器了?难不成巡线就只能靠暴力巡线了?那岂不一个参数没对直接寄了?

此时唯一的传感参数就是光电传感器的光电值

能利用的数据有:

  1. 变速后的速度

  2. 一侧光电识别到黑和另一侧光电识别到黑的时间差

那么要么是利用时间t,要么利用时间t * 速度v

但是用t * v这不还是到了暴力巡线那步,甚至不好估算误差也有可能是我太菜没想到对应算法

那么只能利用时间t来构成一个闭环系统了

原理

对于一个高斯函数

$$Tc=ae^{\frac{-(Ti-b)^2}{2c^2}}$$

在这里,我们定义一侧识别到黑线后向另一边转动Ti(ms)后直行,从直行到下一次识别到黑线的时间差为Tc(ms),若在Ti以内识别到黑线则定义此时Tc为1ms

算法思路

定义Ti每次的增量DeltaTi

采集两次初始{Ti,Tc},判断Tc值

如果Tc两次都为1

  • 如果两次都是同侧且和黑线同侧,则下一次Ti取两次Ti的最大值 + DeltaTi
  • 如果两次都是同侧且和黑线异侧,则下一次Ti取两次Ti的最小值 - DeltaTi
  • 如果两次是异侧,则下一次Ti取这两次Ti的中值,同时DeltaTi = 两次Ti差值的n分

如果Tc三次都为1

  • 参考二分查找,记得减小DeltaTi

反正就是一个循环+递归来进行查找Tc峰值的过程

只要Tc不为1了,那就用这里的Ti对两个相近的Ti进行二分查找

如果查不到Tc(即Tc = 1),那么用新的两个Ti与找到的Tc不为1的Ti同样二分查找,直到找到一个Tc也不为1的Ti

妈的一个二分查找被我叨叨这么久

只要找到了三个Tc不为1的Ti,就可以愉快地…继续二分查找了…

理论上来说最后可以逼近峰值

(可以找个长点的直线,然后用串口通信测量这些数据,这样得到的Ti和DeltaTi就可以直接作为以后的初始数据让今后的巡线更加迅速)

源代码

还有许许多多的部分还没有写完,hmmmm,希望不会烂尾

点我进入Gayhub仓库

使用 Hugo 构建
主题 StackJimmy 设计,DoubleCat 修改
© Copyright Licensed under CC BY-NC-SA 4.0