Python：如何“归零”或“去皮”三维旋转坐标？

Question

（Python）我正在使用一个IMU（惯性测量单元），它连接到一个树莓pi上，它提供[滚动、俯仰、偏航]的角度。在

芯片似乎有一个硬件“零”，无法校准，因此在软件方面我想。在

也就是说，当我打开芯片时，我可能会得到theta = [10, 5, -80]*的读数，并希望将其设置为我的“零”。 *角度在+-180度范围内，如果更容易的话，可以是rad

我有一个粗糙的解决办法，但我追求的是更优雅的：

tare = [*read initial angles*] # This only gets set once
    #inside a loop
    theta = [*read the imu*]
    deg = map(operator.sub, theta, tare) #zero correction, deg = actual (theta) - 'zero' (tare)
    for d in deg: # if angle > 180 then put angle in terms of +_ 180
    if d > 180: d = d - 360
    elif d  < -179: d = d + 360
    print deg

我已经研究了一段时间了，有些人提到了四元数和旋转矩阵，但我无法理解。 任何帮助都将不胜感激！在

下面是一个链接，指向某人在c++中实现它的链接，我用这个库从IMU获取数据： Link

其他可能有用的（stackoverflow）链接： Link

Answer 1

单向，将皮重角度从euler映射到旋转矩阵。（如果你喜欢的话，也可以转换成一个四元数）然后反转你的旋转矩阵（这将是它的转置）——作为rotMatA。取你的欧拉，把它映射到一个腐烂的垫子上（作为rotMatB），然后用你的皮重腐烂图乘以它。如果你愿意的话，你可以从那里回到欧拉那里

rotMatA =eulerToRotMat(tareAngles).transpose()

rotMatB = eulerToRotMat( incomingAngles)

netResult= rotMatToEuler( rotMatA * rotMatB)

从库中获取rotMat到Euler的转换（更好），或者使用类似于http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToMatrix/的源代码实现自己的转换

Answer 2

你可以像这样做得更简洁一点：

def correction(args):
    d = args[0] - args[1]
    return d-360 if d > 180 else d+360 if d < -179 else d

tare = [10, 5, -80]

#inside the loop
theta = [10, 5, -80]
print map(correction, zip(tare, theta))  #  > [0, 0, 0]