目录
- 1. 简介
- 2. 仅有XY偏移的修正过程
- 3. 3D修正过程
1. 简介
一般，我们期望用IMU测量某个Target坐标系的加速度、角速度信息，然而IMU的坐标系与Target 坐标系一般存在位姿关系，此时IMU测量的加速度不能直接代表Target左坐标系的加速度。比如Target 坐标系绕Z轴自转动，Target的XY轴的加速度应该为0；然而IMU的加速度，包含了向心力，XY的加速度发生变化，不能直接用于Target坐标系。- 基于上述问题，如果需要Target坐标系的加速度，需要对IMU的加速度进行向心力的补偿。
- 参考：CSND-浅谈加速度计旋转补偿【3D个人觉得不合理】
2. 仅有XY偏移的修正过程
-
简介：假如
IMU的坐标系与Target坐标系只有平移关系，没有旋转关系，也就是坐标系对齐，且只考虑XY。
-
向心力 F 向 = m a 向 = m w 2 r F_{向}=ma_{向}=mw^2r F向=ma向=mw2r，其中 w w w 为
Target坐标系自转角速度(弧度)。可以得到在Target坐标系下，IMU的向心加速度为
a 向心 = w 2 ⋅ r = w 2 ⋅ △ x 2 + △ y 2 a_{向心}=w^2\cdot r=w^2 \cdot \sqrt {\triangle x^2 + \triangle y^2} a向心=w2⋅r=w2⋅△x2+△y2 -
IMU的X轴上，向心力叠加量为，这一部分需要IMU的X轴加速度减去
X a = a 向心 ⋅ c o s θ = a 向心 ⋅ △ x r = w 2 ⋅ △ x X_a=a_{向心} \cdot cos\theta=a_{向心} \cdot \frac{\triangle x}{r}=w^2\cdot \triangle x Xa=a向心⋅cosθ=a向心⋅r△x=w2⋅△x -
IMU的Y轴上，向心力叠加量为，这一部分需要IMU的Y轴加速度减去
Y a = a 向心 ⋅ s i n θ = a 向心 ⋅ △ y r = w 2 ⋅ △ y Y_a=a_{向心} \cdot sin\theta=a_{向心} \cdot \frac{\triangle y}{r}=w^2\cdot \triangle y Ya=a向心⋅sinθ=a向心⋅r△y=w2⋅△y
3. 3D修正过程
-
首先将IMU的坐标系与Target坐标系的
先旋转修正，此时IMU坐标系与Target的坐标系朝向对齐，问题就可以简化问题 -
对IMU的数据进行旋转
- 设IMU坐标系在Target坐标系
朝向的旋转关系为 R,即Target到IMU的过渡旋转，或者叫在Target坐标系下，IMU的姿态为R
T a r g e t ⋅ R = I M U Target \cdot R= IMU Target⋅R=IMU - 那么对齐后的IMU数据为
a c c = R ⋅ a c c i m u g y r o = R ⋅ g y r o i m u acc=R \cdot acc_{imu} \\ gyro=R\cdot gyro_{imu} acc=R⋅accimugyro=R⋅gyroimu
- 设IMU坐标系在Target坐标系
-
此时问题和第2节中类似，不过此时我们
区别是，需要考虑Z轴的偏移影响- 在第2节可以看到，
Z轴旋转 只会 增加X轴、Y轴的向心力，不会增加Z轴。同理，X轴旋转也会增加Y轴、Z轴向心力，Y轴旋转会增加X轴、Z轴向心力。 - 所以，
每个轴的向心力会受到另外两个轴的旋转影响。这里角速度为Target坐标轴角速度，不过它和旋转后的IMU角速度一致。 - 那么向心力在每个轴的分量为，需要注意的是，
这是原始测量值需要减去的值
a x = w z ⋅ △ x + w y ⋅ △ x a y = w z ⋅ △ y + w x ⋅ △ y a z = w y ⋅ △ z + w x ⋅ △ z a_x=w_z\cdot \triangle x + w_y \cdot \triangle x \\ a_y=w_z\cdot \triangle y + w_x \cdot \triangle y \\ a_z=w_y\cdot \triangle z + w_x \cdot \triangle z \\ ax=wz⋅△x+wy⋅△xay=wz⋅△y+wx⋅△yaz=wy⋅△z+wx⋅△z
- 在第2节可以看到，
-
代码实现
def IMU_Centripetal_force_correction(self, imu_acc, imu_gyro, rotation, position):'''Target坐标系作为参考坐标系,IMU的姿态为rotation、position,进行向心力去除imu_acc、imu_gyro 为IMU的测量值'''# 0. 得到偏移量delta_x, delta_y, delta_z = position[0], position[1], position[2]# 1. 将IMU的坐标系朝向与Target坐标系对齐acc = np.asarray(rotation,dtype=np.float32) @ np.asarray(imu_acc, dtype=np.float32)gyro = np.asarray(rotation,dtype=np.float32) @ np.asarray(imu_gyro, dtype=np.float32)# 2. 向心力的叠加量计算wx, wy, wz = gyro[0], gyro[1], gyro[2]delta_ax = wz*delta_x + wy*delta_xdelta_ay = wz*delta_y + wx*delta_ydelta_az = wy*delta_z + wx*delta_z# 3. 修正加速度，陀螺仪不需要再修正acc[0] = acc[0] - delta_axacc[1] = acc[1] - delta_ayacc[2] = acc[2] - delta_az return acc, gyro
