计算极坐标中的旋转差

Question

我在减去两个极坐标旋转时遇到了问题。问题是我的车什么时候应该通过“6 点钟”轮换。据我观察，朝向 X 轴的旋转等于 0，朝向负 X 轴等于 +-pi。 （编辑：X 轴是垂直轴）

我得到了汽车的当前旋转"psi"和目标位置的极坐标。如果不发生此问题，我无法减去它们。

Answer 1

我通过使用 rem(u,2*pi) 标准化汽车的旋转来解决它（不知道为什么不是从一开始），然后在从 GOAL 旋转中减去 CURRENT 后也使用 rem(u,pi)（所以它不会' t 超调）。