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

合集 - ChatGPT集锦(6)

1.HRTF和BRIR之间的区别和联系2024-09-202.外肢体与协作机器人，移动机器人，外骨骼机器人的区别2024-05-213.MyCobot320Pi2022默认启动ROS1,如何换成ROS22024-10-254.【ROS2】控制机械臂运动，打印机械臂末端位姿2024-10-28

5.用3个IMU计算空间中两个点的相对位置2024-12-23

6.get_coords耗时大于20ms, 累积延迟较大01-19

收起

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. 带入实数计算例子

1. 先获得每个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]$$

2. 将线加速度转换到全局(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]$$

3. 重力补偿：
   从$\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]$$

4. 积分得到速度和位置：
   假设积分的时间步长为$\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]$$

5. 计算相对位置：
   计算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)