这是一个基于 TEB (Timed Elastic Band) 算法实现的轨迹优化器核心代码。