1. 总思路
用两个IMU(1-head，2-robotend)分别固定在想要测量的两个空间点上，用另外一个IMU(3)固定在静止物体上，充当参考坐标系。
IMU1 is mounted on the human head, IMU2 is mounted on the robot end-effector, and IMU3 is mounted on the desk as stationary.
2. 带入实数计算例子
-
先获得每个IMU的线加速度(Linear Acceleration)的数值：下面简写为LA
\[\overrightarrow{LA}_1=[0.2,0.3,9.9] \]\[\overrightarrow{LA}_2=[0.1,0.4,9.7] \]\[\overrightarrow{LA}_3=[0, 0, 9.81] \]获得每个IMU方向四元数(Orientation Quaternion)：下面简写为Rotation (R)
\[R_1=[0, 0, 0.7071,0.7071] \]
以四元数形式绕 Z 轴旋转 90°以四元数形式绕 Y 轴旋转 90°
\[R_2=[0,0.7071,0,0.7071] \]全局的identity四元数
\[R_3=[0, 0, 0, 1] \] -
将线加速度转换到全局(global)坐标系中：
\[R_3=[0, 0, 0, 1] \]
使用静止IMU(3)的方向四元数来定义全局坐标系；
\[R_{1relative}=R_3\cdot{R_1^{-1}} \]
以IMU1为例，首先计算IMU1相对IMU3的旋转：其中，\(R_1^{-1}\)是指四元数\(R_1\)的共轭，也就是\(R_1\)中的x, y, z取相反数，即\(R_1^{-1}=[0, 0, -0.7071,0.7071]\)
\[R_{1relative}=[0, 0, 0, 1]\cdot{[0, 0, -0.7071,0.7071]}=[0, 0, -0.7071,0.7071] \]
带入计算得到用以上旋转来转换IMI1的线性加速度到全局坐标系中，
\[\overrightarrow{LA}_{1}^{quaternion}=[0.2,0.3,9.9,0] \]
先将线性加速度表示为标量部分为0的四元数（从三位变成四位）然后使用上面计算得到的IMU1相对IMU3的相对四元数\(R_{1relative}\)对IMU1的加速度进行旋转：
\[\overrightarrow{LA}_{1global}=R_{1relative}\cdot{\overrightarrow{LA}_{1}^{quaternion}}\cdot{R_{1relative}^{-1}} \]\[\overrightarrow{LA}_{1global}=[0, 0, -0.7071,0.7071]\cdot{[0.2,0.3,9.9,0]}\cdot{[0, 0, 0.7071,0.7071]} \]\[\overrightarrow{LA}_{1global}=[0.3, -0.2, 9.9, 0] \]其中的矢量部分\(\overrightarrow{LA}_{1global}=[0.3, -0.2, 9.9]\)就是全局坐标系中IMU1的线性加速度。
\[\overrightarrow{LA}_{2global}=[-9.7,0.4,0.1] \]
同理，计算得到 -
重力补偿：
\[\overrightarrow{LA}_{1globalG}=\overrightarrow{LA}_{1global}-\vec{g}=[0.3, -0.2, 9.9]-[0,0,9.81]=[0.3, -0.2, 0.09] \]\[\overrightarrow{LA}_{2globalG}=\overrightarrow{LA}_{2global}-\vec{g}=[-9.7,0.4,0.1]-[0,0,9.81]=[-9.7,0.4, -9.71] \]
从\(\overrightarrow{LA}_{global}\)中减去重力加速度\(\overrightarrow{g}=[0,0,9.81]\),得到\(\overrightarrow{LA}_{globalG}\), IMU1和IMU2分别为： -
积分得到速度和位置：
\[\overrightarrow{v(t)}=\int{\overrightarrow{LA}_{globalG}(t)dt} \]
假设积分的时间步长为\(\Delta{t}=0.1\)seconds.
4.1 对加速度进行积分得到速度：为了简化，将积分约写成：
\[\overrightarrow{v}=\overrightarrow{LA}_{globalG}\Delta{t} \]对于IMU1：
\[\overrightarrow{v}_1=[0.3, -0.2, 0.09]\cdot{0.1}=[0.03, -0.02, 0.009] \]对于IMU2：
\[\overrightarrow{v}_1=[-9.7,0.4, -9.71]\cdot{0.1}=[-0.97,0.04, -0.971] \]4.2 对速度进行积分得到位置：
\[\overrightarrow{p(t)}=\int{\overrightarrow{v(t)}dt} \]约写成：
\[\overrightarrow{p}=\overrightarrow{v}\cdot{\Delta{t}} \]对于IMU1：
\[\overrightarrow{p}_1=\overrightarrow{v}_1\cdot{\Delta{t}}=[0.03, -0.02, 0.009]\cdot{0.1}=[0.003, -0.002, 0.0009] \]对于IMU2：
\[\overrightarrow{p}_2=\overrightarrow{v}_2\cdot{\Delta{t}}=[-0.97,0.04, -0.971]\cdot{0.1}=[-0.097,0.004, -0.0971] \] -
计算相对位置：
\[\overrightarrow{p}_{1relative}=\overrightarrow{p}_1-\overrightarrow{p}_3=\overrightarrow{p}_1-[0, 0, 0]=[0.003, -0.002, 0.0009] \]
计算IMU1和IMU2分别相对于IMU3的位置（IMU3静止不动）：
对于IMU1：对于IMU2：
\[\overrightarrow{p}_{2relative}=\overrightarrow{p}_2-\overrightarrow{p}_3=\overrightarrow{p}_1-[0, 0, 0]=[-0.097,0.004, -0.0971] \]IMU1和IMU2之间的相对位置为：
\[\overrightarrow{p}_{relative}=\overrightarrow{p}_{2relative}-\overrightarrow{p}_{1relative} \]\[\overrightarrow{p}_{relative}=[-0.097,0.004, -0.0971]-[0.003, -0.002, 0.0009] \]\[\overrightarrow{p}_{relative}=[-0.1, 0.006, -0.098] \]
4. python实现
import numpy as np
def quaternion_conjugate(q):
"""Compute the conjugate of a quaternion."""
x, y, z, w = q
return (-x, -y, -z, w)
def quaternion_multiply(q1, q2):
"""Multiply two quaternions."""
x1, y1, z1, w1 = q1
x2, y2, z2, w2 = q2
x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
y = w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2
z = w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2
w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
return x, y, z, w
def rotate_vector_by_quaternion(vector, quaternion):
"""Rotate a vector using a quaternion."""
vector_quaternion = (*vector, 0) # Convert vector to quaternion form
q_conjugate = quaternion_conjugate(quaternion)
temp_result = quaternion_multiply(quaternion, vector_quaternion)
rotated_vector_quaternion = quaternion_multiply(temp_result, q_conjugate)
return rotated_vector_quaternion[:3] # Return only the vector part
# Step 1: Define inputs
# Linear accelerations in local IMU frames
LA1 = [0.2, 0.3, 9.9] # IMU1 acceleration
LA2 = [0.1, 0.4, 9.7] # IMU2 acceleration
LA3 = [0.0, 0.0, 9.81] # IMU3 acceleration (stationary)
# Quaternions (rotations)
R1 = [0, 0, 0.7071, 0.7071] # IMU1 orientation (90° rotation about Z-axis)
R2 = [0, 0.7071, 0, 0.7071] # IMU2 orientation (90° rotation about Y-axis)
R3 = [0, 0, 0, 1] # IMU3 orientation (identity quaternion)
# Step 2: Transform accelerations to the global frame
# For IMU1
R1_relative = quaternion_multiply(R3, quaternion_conjugate(R1)) # Relative quaternion for IMU1
LA1_global = rotate_vector_by_quaternion(LA1, R1_relative)
# For IMU2
R2_relative = quaternion_multiply(R3, quaternion_conjugate(R2)) # Relative quaternion for IMU2
LA2_global = rotate_vector_by_quaternion(LA2, R2_relative)
# Step 3: Gravity compensation
g = [0, 0, 9.81] # Gravity vector
LA1_globalG = np.subtract(LA1_global, g)
LA2_globalG = np.subtract(LA2_global, g)
# Step 4: Integration to compute velocity and position
dt = 0.1 # Time step
# Velocity
v1 = np.multiply(LA1_globalG, dt)
v2 = np.multiply(LA2_globalG, dt)
# Position
p1 = np.multiply(v1, dt)
p2 = np.multiply(v2, dt)
# Step 5: Compute relative positions
p1_relative = p1 # Relative to stationary IMU3
p2_relative = p2 # Relative to stationary IMU3
p_relative = np.subtract(p2_relative, p1_relative) # Relative position between IMU1 and IMU2
# Output results
print("IMU1 Global Acceleration:", LA1_global)
print("IMU2 Global Acceleration:", LA2_global)
print("IMU1 Gravity-Compensated Acceleration:", LA1_globalG)
print("IMU2 Gravity-Compensated Acceleration:", LA2_globalG)
print("IMU1 Velocity:", v1)
print("IMU2 Velocity:", v2)
print("IMU1 Position:", p1_relative)
print("IMU2 Position:", p2_relative)
print("Relative Position (IMU2 - IMU1):", p_relative)