用3個IMU計算空間中兩個點的相對位置
1. 總思路
用兩個IMU(1-head，2-fingertip)分別固定在想要測量的兩個空間點上，用另外一個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)
以四元數形式繞 Z 軸旋轉 90°\[R_1=[0, 0, 0.7071,0.7071] \]以四元數形式繞 Y 軸旋轉 90°
\[R_2=[0,0.7071,0,0.7071] \]全局的identity四元數
\[R_3=[0, 0, 0, 1] \] -
將線加速度轉換到全局(global)坐標系中：
使用靜止IMU(3)的方向四元數\[R_3=[0, 0, 0, 1] \]來定義全局坐標系；
以IMU1為例，首先計算IMU1相對IMU3的旋轉：\[R_{1relative}=R_3\cdot{R_1^{-1}} \]其中，\(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的線性加速度到全局坐標系中，
先將線性加速度表示為標量部分為0的四元數（從三位變成四位）\[\overrightarrow{LA}_{1}^{quaternion}=[0.2,0.3,9.9,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}_{global}\)中減去重力加速度\(\overrightarrow{g}=[0,0,9.81]\),得到\(\overrightarrow{LA}_{globalG}\), IMU1和IMU2分別為：\[\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] \] -
積分得到速度和位置：
假設積分的時間步長為\(\Delta{t}=0.1\)seconds.
4.1 對加速度進行積分得到速度：\[\overrightarrow{v(t)}=\int{\overrightarrow{LA}_{globalG}(t)dt} \]為了簡化，將積分約寫成：
\[\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] \] -
計算相對位置：
計算IMU1和IMU2分別相對于IMU3的位置（IMU3靜止不動）：
對于IMU1：\[\overrightarrow{p}_{1relative}=\overrightarrow{p}_1-\overrightarrow{p}_3=\overrightarrow{p}_1-[0, 0, 0]=[0.003, -0.002, 0.0009] \]對于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)
浙公網安備 33010602011771號