修复了vector溢出的错误，修复了尾节点绘制的错误，加入了速度选择的功能，修改了部分函数
Signed-off-by: Chang4 <13750046342@163.com>