说明：  自写曲率计算函数 ,仿真效果非常好，是路径规划的实用方法。
( Since writing the curvature calculation function, Simulation of the effect is very good, Is a practical method of path planning.)