一个自己实现的小循迹算法

一个当时最直觉的想法就是移动车辆的循迹控制与车子当前的定位和路径的横向误差E和车辆当前角度与路径角度的差THELTA_E有关。

拿我们经常用到的车辆模型来说：本能的直觉告诉我车辆离路径越远那么车辆转角控制的量就越大，同理车辆当前角度与路径角度的差越大，那么车辆的控制量也就越大。

THELTA_E一般定义：

THELTA_E = THELTA_VEHICLE - THELTA_PATH

横向误差E定义

E = distance_min(pos_vehicle, path)

控制指令的计算

steering =k * (DELTA_THELTA + PI / 2 *((exp(c * E) - 1) / (exp(c * E) + 1)))

k是比例系数，c为E的敏感系数。