TEB总结

Non-holonomic kinematics：

如上图可视，我们认为每两个离散位姿之间是通过一个圆弧运动的，所以这两个夹角相等，这里使用叉乘来表示等式约束

曲率约束：

速度和加速度约束：