基于四元数的加速度数据重力去除结果异常问题排查

我尝试从加速度数据中去除重力，手中的四元数为w、x、y、z形式，实现代码如下：

import numpy as np
from math import sqrt

# acc - 加速度计数据
# g - 重力向量 g = (0, 0, 9.81)
# q - 姿态四元数
# 思路：将重力向量旋转到当前姿态坐标系，再从加速度数据中减去它
def subtract_gravity(acc, g, q):
    # 将重力向量旋转到当前姿态q，通过 q * g * q_conjugate 实现
    g_rotated = rotate(g, q)
    # 从acc中减去旋转后的g
    return np.subtract(acc, g_rotated)

# 旋转操作需要执行 qgq_conjugate
def rotate(g, q):
    # 将重力向量转换为四元数
    q_g = (0.0,) + g
    # 将重力向量旋转到当前姿态q，执行 q * g * q_conjugate
    return q_mult(q_mult(q, q_g), q_conjugate(q))[1:]

# 归一化四元数 - 模长应为1
def normalize(q, tolerance=0.00001):
    mag2 = sum(n * n for n in q)
    if abs(mag2 - 1.0) > tolerance:
        mag = sqrt(mag2)
        q = tuple(n / mag for n in q)
    return q

def q_mult(q1, q2):
    w1, x1, y1, z1 = q1
    w2, x2, y2, z2 = q2
    w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
    x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
    y = w1 * y2 + y1 * w2 + z1 * x2 - x1 * z2
    z = w1 * z2 + z1 * w2 + x1 * y2 - y1 * x2
    return w, x, y, z

def q_conjugate(q):
    w, x, y, z = q
    return (w, -x, -y, -z)

a = [9.1, -3.7, -0.03]
b = [0.12, 0.79, 0.56, 0.21]
acc = np.array(a)
q = np.array(b)
g = (0, 0, 9.81)
output = subtract_gravity(acc, g, q)

但输出结果不符合预期：我将IMU放置为重力主要作用于X轴，此时去除重力后X轴数据应接近0，但代码输出为[4.48 -4.25 8.61]。请问我的逻辑是否存在错误？

问题分析与修正 #

你的代码存在两个核心问题：

1. 未对输入四元数做归一化
   虽然你实现了normalize函数，但在实际调用时没有对输入的四元数q执行归一化操作。四元数必须是单位四元数才能正确表示旋转，即使输入值接近单位长度，微小的误差也会导致旋转结果偏移。

2. 旋转方向搞反
   四元数旋转的方向取决于姿态的定义：

   • 若你的四元数q表示物体坐标系相对于世界坐标系的姿态，那么要将世界坐标系下的重力向量转换到物体坐标系，正确的旋转公式应为 q* * v * q（q*是q的共轭），而非你当前使用的q * v * q*。
   • 你当前的旋转操作是把物体坐标系的向量转换到世界坐标系，这与你的需求完全相反，导致旋转后的重力向量不是IMU坐标系下的重力分量，自然无法正确抵消。

修正后的代码 #

import numpy as np
from math import sqrt

def subtract_gravity(acc, g, q):
    # 先归一化四元数
    q_normalized = normalize(q)
    g_rotated = rotate(g, q_normalized)
    return np.subtract(acc, g_rotated)

def rotate(g, q):
    q_g = (0.0,) + g
    # 修改旋转顺序：共轭q 乘 向量四元数 乘 q
    return q_mult(q_mult(q_conjugate(q), q_g), q)[1:]

def normalize(q, tolerance=0.00001):
    mag2 = sum(n * n for n in q)
    if abs(mag2 - 1.0) > tolerance:
        mag = sqrt(mag2)
        q = tuple(n / mag for n in q)
    return q

def q_mult(q1, q2):
    w1, x1, y1, z1 = q1
    w2, x2, y2, z2 = q2
    w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
    x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
    y = w1 * y2 + y1 * w2 + z1 * x2 - x1 * z2
    z = w1 * z2 + z1 * w2 + x1 * y2 - y1 * x2
    return w, x, y, z

def q_conjugate(q):
    w, x, y, z = q
    return (w, -x, -y, -z)

a = [9.1, -3.7, -0.03]
b = [0.12, 0.79, 0.56, 0.21]
acc = np.array(a)
q = np.array(b)
g = (0, 0, 9.81)
output = subtract_gravity(acc, g, q)
print(output)

修正后，旋转后的重力向量会正确对应IMU坐标系下的重力分量，减去后就能得到符合预期的线性加速度数据。

内容的提问来源于stack exchange，提问作者abhishek