用atan2在圆上获取角度进行机器人仿真

Question

我正在一个机器人上做一个仿真，它用一个中心坐标(x1，y1)做圆形运动。机器人定期测量其位置并返回信息。假设返回的坐标是(x3，y3)。但是，我不太确定如何在python中使用atan2来计算当前测量值与中心的最后测量值之间的角度。

到目前为止，我正在做以下工作，并想确认它是否正确：

current_angle = atan2(y3-y1, x3-x1)
last_angle = atan2(y2-y1, x2-x1)
angle_difference =  current_angle - last_angle

然后用angle_difference计算角度velocity = angle_difference / dt (dt是上次测量到当前测量之间的时间)。

Answer 1

几乎是正确的。给出的公式在穿过第一象限和第四象限与第二象限和第三象限之间的轴时会有困难，所以你需要取角度差的绝对值的最小值，然后补偿符号。