一、代码示例
def fix_range(x):
if x == 0:
return x
x %= 2*np.pi*x/abs(x)
x -= 2*np.pi*x/abs(x) if abs(x)>np.pi else 0
return x
二、推理过程：
我这里将转换过程分为了两步：
- 先转到[-2pi, 2pi]
如果原弧度是正，则转到[0,2*pi]:
if x>=0:
x = x%(2*np.pi)
如果是负，转到[0,-2*pi]:
if x<0:
x = x%(-2*np.pi)
当x!=0， 则可以用*x/abs(x)表示x的正负。
合并后可去掉条件判断：x %= 2*np.pi*x/abs(x) 。
-
再从[-2pi, 2pi] 到 [-pi, pi]
如果x>pi，那么它应该被修正到x-2pi；
如果x<-pi，应该修正到x+2pi。
依然是用*x/abs(x)影响式子的正负， 且用 x的绝对值跟pi比较来合并上述两个条件。
合并后得到：x -= 2*np.pi*x/abs(x) if abs(x)>np.pi else 0
三、问题扩展
3.1 引入
上述问题其实是一个简单问题。
我最初的想法其实是对任意两个yaw角插值：
用线性插值就是：
- 求两个yaw的差值delta_yaw， 和两个yaw的时间差delta_t;
- yaw_rate = delta_yaw/delta_t;
- result = yaw1 + yaw_rate*(result_time-t1)
在驾驶场景，目标的yaw角取值范围是[-pi, pi]， 作为一个圆， -pi和pi其实一样的，而且在此处取值连续；但是在数值上，-pi和pi差了2pi。
这会造成一个问题：对于面对我们的目标，它的yaw角接近pi，如果前后的yaw角一个是接近pi的正数，一个是接近-pi的负数，我们期望的线性插值结果应该也是pi附近，但是如果直接用上述方法插值，会得到一个接近0的结果，相当于目标调了个头。
3.2 一个可行解法
一个想法是先转成四元数，然后求delta_yaw:
rot_1 = Quaternion(axis=(0.0, 0.0, 1.0), radians=yaw_1).rotation_matrix
rot_2 = Quaternion(axis=(0.0, 0.0, 1.0), radians=yaw_2).rotation_matrix
delta_rot = rot_2 @ np.linalg.inv(rot_1)
yaw, _, _ = Quaternion(matrix=delta_rot).yaw_pitch_roll
但这样做总觉得舍近求远。
3.3 简单解法
于是分情况讨论：
- 出现上述情况其实主要是面对我们的目标，它们的yaw角以pi为中心左右摆动；
- 这种情况下，delta_yaw的绝对值接近2pi，假设把条件再放宽，可得到 abs(delta_yaw)>pi。
例如，yaw_1 = 170° ， yaw_2 = -175° (用角度表示利于理解）。
delta_yaw = yaw_2 - yaw_1 = 345°, 但实际它应该是 15° （逆时针为正）。
其实就是： 360-abs(yaw_2-yaw_1)。
考虑反过来的情况，yaw_1 = -170° ， yaw_2 = 175°， 差值应该是-15°。
其实就是：abs(yaw_2-yaw_1)-360.
以上，我们可以用代码表示为：
delta_yaw = yaw_2 - yaw_1
if abs(delta_yaw)>np.pi:
delta_yaw = (abs(delta_yaw)-2*np.pi)*(delta_yaw/abs(delta_yaw))
式子右边整理得到：
delta_yaw = delta_yaw-2*np.pi*delta_yaw/abs(delta_yaw)
即：
if abs(delta_yaw)>np.pi:
delta_yaw -= 2*np.pi*delta_yaw/abs(delta_yaw)
该表达式即上述的fix_range函数第二步的表达式。而delta_yaw本来就满足第一步处理过后的条件。
也就是说，两个yaw角的差值，也可以用上述fix_range函数修正为我们期望的值。
最后，经过实验，我发现两个yaw角即使不满足在[-pi, pi]取值，任意取值，他们的差值也能通过上述函数得到期望差值。
3.4 结论
对任意两个yaw角插值问题，首先在第一步求delta_yaw时调用fix_range函数，最终在正常差值得到yaw角之后，再调用一次fix_range函数对结果进行取值范围修正即可。
