用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)

posted @ 2024-12-23 18:17 susiezsf 阅读(149) 评论(0) 收藏举报

刷新页面返回顶部

SusieZ

用3个IMU计算空间中两个点的相对位置

1. 总思路

2. 带入实数计算例子

4. python实现

公告